From 9ed1098dc2d853e41b5adc9f99a67464843f4d08 Mon Sep 17 00:00:00 2001 From: psbiomech Date: Mon, 27 Jun 2022 15:04:22 +1000 Subject: [PATCH 1/8] Create .gitkeep --- Bindings/Python/examples/Moco/example2DWalking/.gitkeep | 1 + 1 file changed, 1 insertion(+) create mode 100644 Bindings/Python/examples/Moco/example2DWalking/.gitkeep diff --git a/Bindings/Python/examples/Moco/example2DWalking/.gitkeep b/Bindings/Python/examples/Moco/example2DWalking/.gitkeep new file mode 100644 index 0000000000..8b13789179 --- /dev/null +++ b/Bindings/Python/examples/Moco/example2DWalking/.gitkeep @@ -0,0 +1 @@ + From 4156ba19ecf2e48444899ea1664ac49d433c0f95 Mon Sep 17 00:00:00 2001 From: psbiomech Date: Mon, 27 Jun 2022 15:07:24 +1000 Subject: [PATCH 2/8] Add files via upload --- .../Moco/example2DWalking/2D_gait.osim | 2720 +++++++++++++++++ .../Moco/example2DWalking/example2DWalking.py | 384 +++ .../example2DWalkingMetabolics.py | 206 ++ .../example2DWalkingStepAsymmetry.py | 511 ++++ .../example2DWalking/referenceCoordinates.sto | 56 + .../Moco/example2DWalking/referenceGRF.sto | 56 + .../Moco/example2DWalking/referenceGRF.xml | 32 + 7 files changed, 3965 insertions(+) create mode 100644 Bindings/Python/examples/Moco/example2DWalking/2D_gait.osim create mode 100644 Bindings/Python/examples/Moco/example2DWalking/example2DWalking.py create mode 100644 Bindings/Python/examples/Moco/example2DWalking/example2DWalkingMetabolics.py create mode 100644 Bindings/Python/examples/Moco/example2DWalking/example2DWalkingStepAsymmetry.py create mode 100644 Bindings/Python/examples/Moco/example2DWalking/referenceCoordinates.sto create mode 100644 Bindings/Python/examples/Moco/example2DWalking/referenceGRF.sto create mode 100644 Bindings/Python/examples/Moco/example2DWalking/referenceGRF.xml diff --git a/Bindings/Python/examples/Moco/example2DWalking/2D_gait.osim b/Bindings/Python/examples/Moco/example2DWalking/2D_gait.osim new file mode 100644 index 0000000000..a784971254 --- /dev/null +++ b/Bindings/Python/examples/Moco/example2DWalking/2D_gait.osim @@ -0,0 +1,2720 @@ + + + + + + + + /contactgeometryset/heel_r + + /contactgeometryset/floor + + 3067776 + + 2 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + 300 + + 50 + + + + /contactgeometryset/heel_l + + /contactgeometryset/floor + + 3067776 + + 2 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + 300 + + 50 + + + + /contactgeometryset/front_r + + /contactgeometryset/floor + + 3067776 + + 2 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + 300 + + 50 + + + + /contactgeometryset/front_l + + /contactgeometryset/floor + + 3067776 + + 2 + + 0.80000000000000004 + + 0.80000000000000004 + + 0.5 + + 0.20000000000000001 + + 300 + + 50 + + + + -150 + + 150 + + lumbar + + 1 + + + + 0.01 + + 1 + + + + + + + + /bodyset/pelvis + + -0.121645 -0.099055900000000002 0.068467600000000003 + + + + /bodyset/tibia_r + + -0.029098599999999999 -0.034802399999999997 0.028450900000000001 + + + + /bodyset/tibia_r + + -0.022621499999999999 -0.054427000000000003 0.033158899999999998 + + + + + + + + + + + + + 0.80000000000000004 0.10000000000000001 0.10000000000000001 + + + + 2700 + + 0.10905928770854099 + + 0.32617731920169202 + + 0 + + 10 + + true + + false + + 0.014999999999999999 + + 0.059999999999999998 + + 0.01 + + 1 + + 0.01 + + 0.049000000000000002 + + false + + + + 0.01 + + 1 + + + + + + + + /bodyset/femur_r + + 0.0050137300000000001 -0.21167900000000001 0.023464200000000001 + + + + /bodyset/tibia_r + + -0.029098599999999999 -0.034802399999999997 0.028450900000000001 + + + + /bodyset/tibia_r + + -0.022621499999999999 -0.054427000000000003 0.033158899999999998 + + + + + + + + + + + + + 0.80000000000000004 0.10000000000000001 0.10000000000000001 + + + + 804 + + 0.171872304562285 + + 0.088419856104296701 + + 0.40142572999999998 + + 10 + + true + + false + + 0.014999999999999999 + + 0.059999999999999998 + + 0.01 + + 1 + + 0.01 + + 0.049000000000000002 + + false + + + + 0.01 + + 1 + + + + + + + + /bodyset/pelvis + + -0.127188 0.0084019400000000001 0.045553000000000003 + + + + /bodyset/pelvis + + -0.12979499999999999 -0.058813600000000001 0.080161499999999997 + + + + /bodyset/femur_r + + -0.045123499999999997 -0.058560300000000003 0.025269199999999999 + + + + /bodyset/femur_r + + -0.015642799999999998 -0.101879 0.042014999999999997 + + + + + + + + + + + + + 0.80000000000000004 0.10000000000000001 0.10000000000000001 + + + + 1944 + + 0.15477839388148101 + + 0.072308835382489306 + + 0 + + 10 + + true + + false + + 0.014999999999999999 + + 0.059999999999999998 + + 0.01 + + 1 + + 0.01 + + 0.049000000000000002 + + false + + + + 0.01 + + 1 + + + + + + + + /bodyset/pelvis + + -0.062483400000000001 0.085661100000000004 0.028495300000000001 + + + + /bodyset/pelvis + + -0.022984600000000001 -0.055047199999999998 0.074837100000000004 + + + + /bodyset/pelvis + + -0.027813299999999999 -0.077742000000000006 0.080457299999999995 + + /jointset/hip_r/hip_flexion_r + + -1.5708 0.78539800000000004 + + + + /bodyset/femur_r + + 0.00160439 -0.050839200000000001 0.0038104300000000001 + + + + /bodyset/femur_r + + -0.0188516 -0.059863899999999998 0.0104285 + + + + + + + + + + + + + 0.80000000000000004 0.10000000000000001 0.10000000000000001 + + + + 2342 + + 0.097499585002471204 + + 0.15599933600395399 + + 0.13962633999999999 + + 10 + + true + + false + + 0.014999999999999999 + + 0.059999999999999998 + + 0.01 + + 1 + + 0.01 + + 0.049000000000000002 + + false + + + + 0.01 + + 1 + + + + + + + + /bodyset/pelvis + + -0.028489299999999999 -0.030034499999999999 0.095444399999999999 + + + + /bodyset/femur_r + + 0.033491699999999999 -0.40410600000000002 0.00190522 + + /jointset/knee_r/knee_angle_r + + -2.6179899999999998 -1.45997 + + + + /bodyset/tibia_r + + /jointset/knee_r/knee_angle_r + + /jointset/knee_r/knee_angle_r + + /jointset/knee_r/knee_angle_r + + + + + + -2.0944 -1.99997 -1.5708 -1.45752 -1.39626 -1.0472 -0.698132 -0.526391 -0.349066 -0.174533 0 0.00017453 0.00034907 0.0279253 0.0872665 0.174533 2.0944 + 0.0155805 0.0179938 0.0275081 0.0296564 0.0307615 0.0365695 0.0422074 0.0450902 0.048391 0.0534299 0.0617576 0.0617669 0.0617762 0.0633083 0.066994 0.0733035 0.0573481 + + + 0.96673215203466201 + + + + + + + + -2.0944 -1.99997 -1.5708 -1.45752 -1.39626 -1.0472 -0.698132 -0.526391 -0.349066 -0.174533 0 0.00017453 0.00034907 0.0279253 0.0872665 0.174533 2.0944 + 0.0234116 0.0237613 0.0251141 0.0252795 0.0253146 0.0249184 0.0242373 0.0238447 0.0234197 0.0227644 0.020984 0.0209814 0.0209788 0.0205225 0.0191754 0.0159554 -0.0673774 + + + 0.96673215203466201 + + + + + + + + -2.0944 0.1745 + 0.0014 0.0014 + + + 0.96673215203466201 + + + + + + + + + + + + + + + 0.80000000000000004 0.10000000000000001 0.10000000000000001 + + + + 1169 + + 0.114154396713697 + + 0.31041985071268602 + + 0.087266460000000004 + + 10 + + true + + false + + 0.014999999999999999 + + 0.059999999999999998 + + 0.01 + + 1 + + 0.01 + + 0.049000000000000002 + + false + + + + 0.01 + + 1 + + + + + + + + /bodyset/femur_r + + 0.029079600000000001 -0.19292799999999999 0.031085100000000001 + + + + /bodyset/femur_r + + 0.033591999999999997 -0.20897199999999999 0.028578200000000002 + + + + /bodyset/femur_r + + 0.0343942 -0.40410600000000002 0.0055151000000000002 + + /jointset/knee_r/knee_angle_r + + -2.6179899999999998 -1.4199999999999999 + + + + /bodyset/tibia_r + + /jointset/knee_r/knee_angle_r + + /jointset/knee_r/knee_angle_r + + /jointset/knee_r/knee_angle_r + + + + + + -2.0944 -1.99997 -1.5708 -1.45752 -1.39626 -1.0472 -0.698132 -0.526391 -0.349066 -0.174533 0 0.00017453 0.00034907 0.0279253 0.0872665 0.174533 2.0944 + 0.0082733 0.0106866 0.0202042 0.022353 0.0234583 0.0292715 0.0349465 0.037871 0.0412569 0.0465287 0.0554632 0.0554735 0.0554837 0.0571717 0.061272 0.0684368 0.0648818 + + + 0.96673215203466201 + + + + + + + + -2.0944 -1.99997 -1.5708 -1.45752 -1.39626 -1.0472 -0.698132 -0.526391 -0.349066 -0.174533 0 0.00017453 0.00034907 0.0279253 0.0872665 0.174533 2.0944 + 0.025599 0.0259487 0.0273124 0.0274796 0.0275151 0.0271363 0.0265737 0.0263073 0.0261187 0.0260129 0.0252923 0.0252911 0.0252898 0.0250526 0.0242191 0.0218288 -0.0685706 + + + 0.96673215203466201 + + + + + + + + -2.0944 2.0944 + 0.0018 0.0018 + + + 0.96673215203466201 + + + + + + + + + + + + + + + 0.80000000000000004 0.10000000000000001 0.10000000000000001 + + + + 5000 + + 0.107708763662917 + + 0.116768379298115 + + 0.052359879999999998 + + 10 + + true + + false + + 0.014999999999999999 + + 0.059999999999999998 + + 0.01 + + 1 + + 0.01 + + 0.049000000000000002 + + false + + + + 0.01 + + 1 + + + + + + + + /bodyset/femur_r + + -0.019052199999999998 -0.39397900000000002 -0.023564499999999999 + + + + /bodyset/femur_r + + -0.030082399999999999 -0.403304 -0.025870799999999999 + + /jointset/knee_r/knee_angle_r + + -0.78539800000000004 0.17453299999999999 + + + + /bodyset/calcn_r + + 0 0.028331700000000001 -0.0048437999999999997 + + + + + + + + + + + + + 0.80000000000000004 0.10000000000000001 0.10000000000000001 + + + + 2500 + + 0.086926079311056698 + + 0.34770431724422701 + + 0.29670596999999999 + + 10 + + true + + false + + 0.014999999999999999 + + 0.059999999999999998 + + 0.01 + + 1 + + 0.01 + + 0.049000000000000002 + + false + + + + 0.01 + + 1 + + + + + + + + /bodyset/tibia_r + + -0.0023201599999999999 -0.1482 0.0068637999999999998 + + + + /bodyset/calcn_r + + 0 0.028331700000000001 -0.0048437999999999997 + + + + + + + + + + + + + 0.80000000000000004 0.10000000000000001 0.10000000000000001 + + + + 5137 + + 0.048170551124502803 + + 0.24085275562251399 + + 0.43633231 + + 10 + + true + + false + + 0.014999999999999999 + + 0.059999999999999998 + + 0.01 + + 1 + + 0.01 + + 0.049000000000000002 + + false + + + + 0.01 + + 1 + + + + + + + + /bodyset/tibia_r + + 0.0173045 -0.156997 0.0111174 + + + + /bodyset/tibia_r + + 0.0318055 -0.38195600000000002 -0.0171112 + + + + /bodyset/calcn_r + + 0.10656400000000001 0.016267899999999998 -0.027874699999999999 + + + + + + + + + + + + + 0.80000000000000004 0.10000000000000001 0.10000000000000001 + + + + 3000 + + 0.093789916893817799 + + 0.213419912931851 + + 0.087266460000000004 + + 10 + + true + + false + + 0.014999999999999999 + + 0.059999999999999998 + + 0.01 + + 1 + + 0.01 + + 0.049000000000000002 + + false + + + + 0.01 + + 1 + + + + + + + + /bodyset/pelvis + + -0.121645 -0.099055900000000002 -0.068467600000000003 + + + + /bodyset/tibia_l + + -0.029098599999999999 -0.034802399999999997 -0.028450900000000001 + + + + /bodyset/tibia_l + + -0.022621499999999999 -0.054427000000000003 -0.033158899999999998 + + + + + + + + + + + + + 0.80000000000000004 0.10000000000000001 0.10000000000000001 + + + + 2700 + + 0.10905928770854099 + + 0.32617731920169202 + + 0 + + 10 + + true + + false + + 0.014999999999999999 + + 0.059999999999999998 + + 0.01 + + 1 + + 0.01 + + 0.049000000000000002 + + false + + + + 0.01 + + 1 + + + + + + + + /bodyset/femur_l + + 0.0050137300000000001 -0.21167900000000001 -0.023464200000000001 + + + + /bodyset/tibia_l + + -0.029098599999999999 -0.034802399999999997 -0.028450900000000001 + + + + /bodyset/tibia_l + + -0.022621499999999999 -0.054427000000000003 -0.033158899999999998 + + + + + + + + + + + + + 0.80000000000000004 0.10000000000000001 0.10000000000000001 + + + + 804 + + 0.171872304562285 + + 0.088419856104296701 + + 0.40142572999999998 + + 10 + + true + + false + + 0.014999999999999999 + + 0.059999999999999998 + + 0.01 + + 1 + + 0.01 + + 0.049000000000000002 + + false + + + + 0.01 + + 1 + + + + + + + + /bodyset/pelvis + + -0.127188 0.0084019400000000001 -0.045553000000000003 + + + + /bodyset/pelvis + + -0.12979499999999999 -0.058813600000000001 -0.080161499999999997 + + + + /bodyset/femur_l + + -0.045123499999999997 -0.058560300000000003 -0.025269199999999999 + + + + /bodyset/femur_l + + -0.015642799999999998 -0.101879 -0.042014999999999997 + + + + + + + + + + + + + 0.80000000000000004 0.10000000000000001 0.10000000000000001 + + + + 1944 + + 0.15477839388148101 + + 0.072308835382489306 + + 0 + + 10 + + true + + false + + 0.014999999999999999 + + 0.059999999999999998 + + 0.01 + + 1 + + 0.01 + + 0.049000000000000002 + + false + + + + 0.01 + + 1 + + + + + + + + /bodyset/pelvis + + -0.062483400000000001 0.085661100000000004 -0.028495300000000001 + + + + /bodyset/pelvis + + -0.022984600000000001 -0.055047199999999998 -0.074837100000000004 + + + + /bodyset/pelvis + + -0.027813299999999999 -0.077742000000000006 -0.080457299999999995 + + /jointset/hip_l/hip_flexion_l + + -1.5708 0.78539800000000004 + + + + /bodyset/femur_l + + 0.00160439 -0.050839200000000001 -0.0038104300000000001 + + + + /bodyset/femur_l + + -0.0188516 -0.059863899999999998 -0.0104285 + + + + + + + + + + + + + 0.80000000000000004 0.10000000000000001 0.10000000000000001 + + + + 2342 + + 0.097499585002471204 + + 0.15599933600395399 + + 0.13962633999999999 + + 10 + + true + + false + + 0.014999999999999999 + + 0.059999999999999998 + + 0.01 + + 1 + + 0.01 + + 0.049000000000000002 + + false + + + + 0.01 + + 1 + + + + + + + + /bodyset/pelvis + + -0.028489299999999999 -0.030034499999999999 -0.095444399999999999 + + + + /bodyset/femur_l + + 0.033491699999999999 -0.40410600000000002 -0.00190522 + + /jointset/knee_l/knee_angle_l + + -2.6179899999999998 -1.45997 + + + + /bodyset/tibia_l + + /jointset/knee_l/knee_angle_l + + /jointset/knee_l/knee_angle_l + + /jointset/knee_l/knee_angle_l + + + + + + -2.0944 -1.99997 -1.5708 -1.45752 -1.39626 -1.0472 -0.698132 -0.526391 -0.349066 -0.174533 0 0.00017453 0.00034907 0.0279253 0.0872665 0.174533 2.0944 + 0.0155805 0.0179938 0.0275081 0.0296564 0.0307615 0.0365695 0.0422074 0.0450902 0.048391 0.0534299 0.0617576 0.0617669 0.0617762 0.0633083 0.066994 0.0733035 0.0573481 + + + 0.96673215203466201 + + + + + + + + -2.0944 -1.99997 -1.5708 -1.45752 -1.39626 -1.0472 -0.698132 -0.526391 -0.349066 -0.174533 0 0.00017453 0.00034907 0.0279253 0.0872665 0.174533 2.0944 + 0.0234116 0.0237613 0.0251141 0.0252795 0.0253146 0.0249184 0.0242373 0.0238447 0.0234197 0.0227644 0.020984 0.0209814 0.0209788 0.0205225 0.0191754 0.0159554 -0.0673774 + + + 0.96673215203466201 + + + + + + + + -2.0944 0.1745 + -0.0014 -0.0014 + + + 0.96673215203466201 + + + + + + + + + + + + + + + 0.80000000000000004 0.10000000000000001 0.10000000000000001 + + + + 1169 + + 0.114154396713697 + + 0.31041985071268602 + + 0.087266460000000004 + + 10 + + true + + false + + 0.014999999999999999 + + 0.059999999999999998 + + 0.01 + + 1 + + 0.01 + + 0.049000000000000002 + + false + + + + 0.01 + + 1 + + + + + + + + /bodyset/femur_l + + 0.029079600000000001 -0.19292799999999999 -0.031085100000000001 + + + + /bodyset/femur_l + + 0.033591999999999997 -0.20897199999999999 -0.028578200000000002 + + + + /bodyset/femur_l + + 0.0343942 -0.40410600000000002 -0.0055151000000000002 + + /jointset/knee_l/knee_angle_l + + -2.6179899999999998 -1.4199999999999999 + + + + /bodyset/tibia_l + + /jointset/knee_l/knee_angle_l + + /jointset/knee_l/knee_angle_l + + /jointset/knee_l/knee_angle_l + + + + + + -2.0944 -1.99997 -1.5708 -1.45752 -1.39626 -1.0472 -0.698132 -0.526391 -0.349066 -0.174533 0 0.00017453 0.00034907 0.0279253 0.0872665 0.174533 2.0944 + 0.0082733 0.0106866 0.0202042 0.022353 0.0234583 0.0292715 0.0349465 0.037871 0.0412569 0.0465287 0.0554632 0.0554735 0.0554837 0.0571717 0.061272 0.0684368 0.0648818 + + + 0.96673215203466201 + + + + + + + + -2.0944 -1.99997 -1.5708 -1.45752 -1.39626 -1.0472 -0.698132 -0.526391 -0.349066 -0.174533 0 0.00017453 0.00034907 0.0279253 0.0872665 0.174533 2.0944 + 0.025599 0.0259487 0.0273124 0.0274796 0.0275151 0.0271363 0.0265737 0.0263073 0.0261187 0.0260129 0.0252923 0.0252911 0.0252898 0.0250526 0.0242191 0.0218288 -0.0685706 + + + 0.96673215203466201 + + + + + + + + -2.0944 2.0944 + -0.0018 -0.0018 + + + 0.96673215203466201 + + + + + + + + + + + + + + + 0.80000000000000004 0.10000000000000001 0.10000000000000001 + + + + 5000 + + 0.107708763662917 + + 0.116768379298115 + + 0.052359879999999998 + + 10 + + true + + false + + 0.014999999999999999 + + 0.059999999999999998 + + 0.01 + + 1 + + 0.01 + + 0.049000000000000002 + + false + + + + 0.01 + + 1 + + + + + + + + /bodyset/femur_l + + -0.019052199999999998 -0.39397900000000002 0.023564499999999999 + + + + /bodyset/femur_l + + -0.030082399999999999 -0.403304 0.025870799999999999 + + /jointset/knee_l/knee_angle_l + + -0.78539800000000004 0.17453299999999999 + + + + /bodyset/calcn_l + + 0 0.028331700000000001 0.0048437999999999997 + + + + + + + + + + + + + 0.80000000000000004 0.10000000000000001 0.10000000000000001 + + + + 2500 + + 0.086926079311056698 + + 0.34770431724422701 + + 0.29670596999999999 + + 10 + + true + + false + + 0.014999999999999999 + + 0.059999999999999998 + + 0.01 + + 1 + + 0.01 + + 0.049000000000000002 + + false + + + + 0.01 + + 1 + + + + + + + + /bodyset/tibia_l + + -0.0023201599999999999 -0.1482 -0.0068637999999999998 + + + + /bodyset/calcn_l + + 0 0.028331700000000001 0.0048437999999999997 + + + + + + + + + + + + + 0.80000000000000004 0.10000000000000001 0.10000000000000001 + + + + 5137 + + 0.048170551124502803 + + 0.24085275562251399 + + 0.43633231 + + 10 + + true + + false + + 0.014999999999999999 + + 0.059999999999999998 + + 0.01 + + 1 + + 0.01 + + 0.049000000000000002 + + false + + + + 0.01 + + 1 + + + + + + + + /bodyset/tibia_l + + 0.0173045 -0.156997 -0.0111174 + + + + /bodyset/tibia_l + + 0.0318055 -0.38195600000000002 0.0171112 + + + + /bodyset/calcn_l + + 0.10656400000000001 0.016267899999999998 0.027874699999999999 + + + + + + + + + + + + + 0.80000000000000004 0.10000000000000001 0.10000000000000001 + + + + 3000 + + 0.093789916893817799 + + 0.213419912931851 + + 0.087266460000000004 + + 10 + + true + + false + + 0.014999999999999999 + + 0.059999999999999998 + + 0.01 + + 1 + + 0.01 + + 0.049000000000000002 + + false + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 0.96574000000000004 0.96574000000000004 0.98599599999999998 + + + + 1 + + 1 1 1 + + + r_pelvis.vtp + + + + .. + + 0.96574000000000004 0.96574000000000004 0.98599599999999998 + + + + 1 + + 1 1 1 + + + l_pelvis.vtp + + + + .. + + 0.96574000000000004 0.96574000000000004 0.98599599999999998 + + + + 1 + + 1 1 1 + + + sacrum.vtp + + + + 9.7143336091723995 + + -0.0682778 0 0 + + 0.081492884605030597 0.081492884605030597 0.044542759153066699 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 1.00275 1.00275 1.00275 + + + + 1 + + 1 1 1 + + + femur_l.vtp + + + + 7.6723191502382804 + + 0 -0.17046700000000001 0 + + 0.11105547289013901 0.029111628815861601 0.117110028170931 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 1.00275 1.00275 1.00275 + + + + 1 + + 1 1 1 + + + femur_r.vtp + + + + 7.6723191502382804 + + 0 -0.17046700000000001 0 + + 0.11105547289013901 0.029111628815861601 0.117110028170931 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 0.96673200000000004 0.96673200000000004 0.96673200000000004 + + + + 1 + + 1 1 1 + + + tibia_l.vtp + + + + .. + + 0.96673200000000004 0.96673200000000004 0.96673200000000004 + + + + 1 + + 1 1 1 + + + l_fibula.vtp + + + + 3.0581550357482099 + + 0 -0.18048900000000001 0 + + 0.038852699659735403 0.0039315231798541803 0.039392320488342902 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 0.96673200000000004 0.96673200000000004 0.96673200000000004 + + + + 1 + + 1 1 1 + + + tibia_r.vtp + + + + .. + + 0.96673200000000004 0.96673200000000004 0.96673200000000004 + + + + 1 + + 1 1 1 + + + r_fibula.vtp + + + + 3.0581550357482099 + + 0 -0.18048900000000001 0 + + 0.038852699659735403 0.0039315231798541803 0.039392320488342902 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 0.91392399999999996 0.91392399999999996 0.91392399999999996 + + + + 1 + + 1 1 1 + + + l_talus.vtp + + + + 0.082485638186061 + + 0 0 0 + + 0.00068896770091018196 0.00068896770091018196 0.00068896770091018196 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 0.91392399999999996 0.91392399999999996 0.91392399999999996 + + + + 1 + + 1 1 1 + + + r_talus.vtp + + + + 0.082485638186061 + + 0 0 0 + + 0.00068896770091018196 0.00068896770091018196 0.00068896770091018196 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 0.91392399999999996 0.91392399999999996 0.91392399999999996 + + + + 1 + + 1 1 1 + + + l_foot.vtp + + + + 1.03107047732576 + + 0.091392399999999999 0.0274177 0 + + 0.00096455478127425401 0.0026869740335497098 0.0028247675737317502 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 0.91392399999999996 0.91392399999999996 0.91392399999999996 + + + + 1 + + 1 1 1 + + + r_foot.vtp + + + + 1.03107047732576 + + 0.091392399999999999 0.0274177 0 + + 0.00096455478127425401 0.0026869740335497098 0.0028247675737317502 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 0.91392399999999996 0.91392399999999996 0.91392399999999996 + + + + 1 + + 1 1 1 + + + l_bofoot.vtp + + + + 0.17866389231100799 + + 0.031621799999999999 0.0054835500000000002 0.0159937 + + 6.8896770091018202e-05 0.000137793540182036 6.8896770091018202e-05 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 0.91392399999999996 0.91392399999999996 0.91392399999999996 + + + + 1 + + 1 1 1 + + + r_bofoot.vtp + + + + 0.17866389231100799 + + 0.031621799999999999 0.0054835500000000002 -0.0159937 + + 6.8896770091018202e-05 0.000137793540182036 6.8896770091018202e-05 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + + + + .. + + 0.96574000000000004 0.96574000000000004 0.98599599999999998 + + + + 1 + + 1 1 1 + + + hat_spine.vtp + + + + .. + + 0.96574000000000004 0.96574000000000004 0.98599599999999998 + + + + 1 + + 1 1 1 + + + hat_jaw.vtp + + + + .. + + 0.96574000000000004 0.96574000000000004 0.98599599999999998 + + + + 1 + + 1 1 1 + + + hat_skull.vtp + + + + .. + + 0.96574000000000004 0.96574000000000004 0.98599599999999998 + + + + 1 + + 1 1 1 + + + hat_ribs_scap.vtp + + + + 28.240278003208999 + + -0.0289722 0.30903700000000001 0 + + 1.14043571182129 0.593400919285897 1.14043571182129 0 0 0 + + + + + + + + + + ground_offset + + pelvis_offset + + + + + -1.570796326794897 1.570796326794897 + + + + -5 5 + + + + -3 3 + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /ground + + 0 0 0 + + 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/pelvis + + 0 0 0 + + 0 0 0 + + + + + + pelvis_offset + + femur_l_offset + + + + + -1.0471975511965981 2.0943951023931948 + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/pelvis + + -0.068277800171117897 -0.063835397331130098 -0.082330694005868801 + + 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/femur_l + + 0 0 0 + + 0 0 0 + + + + + + pelvis_offset + + femur_r_offset + + + + + -1.0471975511965981 2.0943951023931948 + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/pelvis + + -0.068277800171117897 -0.063835397331130098 0.082330694005868801 + + 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/femur_r + + 0 0 0 + + 0 0 0 + + + + + + femur_l_offset + + tibia_l_offset + + + + + -2.0943951023931948 0.174532925199433 + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/femur_l + + -0.0045122123214679797 -0.39690724592144699 0 + + 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/tibia_l + + 0 0 0 + + 0 0 0 + + + + + + femur_r_offset + + tibia_r_offset + + + + + -2.0943951023931948 0.174532925199433 + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/femur_r + + -0.0045122123214679797 -0.39690724592144699 0 + + 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/tibia_r + + 0 0 0 + + 0 0 0 + + + + + + tibia_l_offset + + talus_l_offset + + + + + -0.69813170079773201 0.52359877559829904 + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/tibia_l + + 0 -0.41569482537490499 0 + + 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/talus_l + + 0 0 0 + + 0 0 0 + + + + + + tibia_r_offset + + talus_r_offset + + + + + -0.69813170079773201 0.52359877559829904 + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/tibia_r + + 0 -0.41569482537490499 0 + + 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/talus_r + + 0 0 0 + + 0 0 0 + + + + + + talus_l_offset + + calcn_l_offset + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/talus_l + + -0.044572091911732101 -0.038339127654237401 -0.0072382810732195598 + + 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/calcn_l + + 0 0 0 + + 0 0 0 + + + + + + talus_r_offset + + calcn_r_offset + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/talus_r + + -0.044572091911732101 -0.038339127654237401 0.0072382810732195598 + + 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/calcn_r + + 0 0 0 + + 0 0 0 + + + + + + calcn_l_offset + + toes_l_offset + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/calcn_l + + 0.16340967877419901 -0.00182784875586352 -0.00098703832816630292 + + 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/toes_l + + 0 0 0 + + 0 0 0 + + + + + + calcn_r_offset + + toes_r_offset + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/calcn_r + + 0.16340967877419901 -0.00182784875586352 0.00098703832816630292 + + 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/toes_r + + 0 0 0 + + 0 0 0 + + + + + + pelvis_offset + + torso_offset + + + + + -1.570796326794897 1.570796326794897 + + + + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/pelvis + + -0.0972499926058214 0.078707789447611198 0 + + 0 0 0 + + + + + + .. + + 0.20000000000000001 0.20000000000000001 0.20000000000000001 + + + /bodyset/torso + + 0 0 0 + + 0 0 0 + + + + + + + + + + + + + + + + + + + + + + /bodyset/calcn_r + + 0.031307527581931796 0.010435842527310599 0 + + 0.035000000000000003 + + + + /bodyset/calcn_l + + 0.031307527581931796 0.010435842527310599 0 + + 0.035000000000000003 + + + + /bodyset/calcn_r + + 0.17740932296428019 -0.015653763790965898 0.0052179212636552993 + + 0.014999999999999999 + + + + /bodyset/calcn_l + + 0.17740932296428019 -0.015653763790965898 -0.0052179212636552993 + + 0.014999999999999999 + + + + /ground + + 0 0 -1.5707963267948966 + + + + + diff --git a/Bindings/Python/examples/Moco/example2DWalking/example2DWalking.py b/Bindings/Python/examples/Moco/example2DWalking/example2DWalking.py new file mode 100644 index 0000000000..182ac5d089 --- /dev/null +++ b/Bindings/Python/examples/Moco/example2DWalking/example2DWalking.py @@ -0,0 +1,384 @@ +# -*- coding: utf-8 -*- +""" +MOCO: WALKING 2D EXAMPLE - TRACKING & PREDICTION + +@author: Prasanna Sritharan, June 2022 + +This is a Python implementation of an example optimal control +problem (2D walking) orginally created in C++ by Antoine Falisse +(see: example2DWalking.cpp) and adapted for Matlab by Brian Umberger +(see: example2DWalking.m). +""" + +from math import pi +import opensim as osim + + + + +""" +gait_tracking(): +Set up a coordinate tracking problem where the goal is to minimize the +difference between provided and simulated coordinate values and speeds (and +ground reaction forces), as well as to minimize an effort cost (squared +controls). The provided data represents one step. Endpoint constraints +enforce periodicity of the coordinate values (except for pelvis tx) and speeds, +coordinate actuator controls, and muscle activations. +""" +def gait_tracking(): + + + + # ********************************** + # DEFINE THE OPTIMAL CONTROL PROBLEM + + # Create tracking problem + track = osim.MocoTrack() + track.setName("gait_tracking") + + # Construct a ModelProcessor and set it on the tool + modelProcessor = osim.ModelProcessor("2D_gait.osim") + track.setModel(modelProcessor) + + + # Get the coordinates into a TableProcessor + tableProcessor = osim.TableProcessor("referenceCoordinates.sto") + tableProcessor.append(osim.TabOpLowPassFilter(6.0)) + + # Set the coordinates as the reference states + track.setStatesReference(tableProcessor) + track.set_allow_unused_references(True) + + # Only coordinates are provided so derive to get speeds and track these too + track.set_track_reference_position_derivatives(True) + + # use coordinates and speeds for initial guess + track.set_apply_tracked_states_to_guess(True) + + # Define the global state tracking weight + track.set_states_global_tracking_weight(1.0) + + + # Set initial time, final time and mesh interval + track.set_initial_time(0.0) + track.set_final_time(0.47008941) + + # Call initialize() to receive a pre-configured MocoStudy object based on the + # settings above. Use this to customize the problem beyond the MocoTrack + # interface. + study = track.initialize() + + # Get a writable reference to the MocoProblem from the MocoStudy to perform + # more customisation + problem = study.updProblem() + + + + # ********************************** + # SET GOALS + + # Symmetry: + # This goal allows us to simulate only one step with left-right symmetry that + # we can then double to create two steps, one on each leg (IFO>IFS>CFO>CFS). + # Note that this is not actually a full gait cycle (IFO>IFS>CFO>CFS>IFO). + symmetryGoal = osim.MocoPeriodicityGoal("symmetry_goal") + problem.addGoal(symmetryGoal) + + # Enforce periodic symmetry + model = modelProcessor.process() + model.initSystem() + state_names = [model.getStateVariableNames().getitem(sn) for sn in range(model.getNumStateVariables())] + for sn in state_names: + + # Symmetric coordinate values and speeds (except for pelvis_tx value): + # Here we constrain final coordinate values of one leg to match the initial + # value of the other leg. Manually add pelvis_tx speed later. + if "jointset" in sn: + if "_r" in sn: + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, sn.replace("_r", "_l"))) + elif "_l" in sn: + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, sn.replace("_l", "_r"))) + elif "pelvis_tx" not in sn: + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn)) + + # Symmetric muscle activations: + # Here, we constrain final muscle activation values of one leg to match the + # initial activation values of the other leg. + elif "activation" in sn: + if "_r" in sn: + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, sn.replace("_r", "_l"))) + elif "_l" in sn: + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, sn.replace("_l", "_r"))) + + + # The pelvis_tx speed has periodic symmetry + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair("/jointset/groundPelvis/pelvis_tx/speed")) + + # Lumbar control has periodic symmetry. The other controls don't need + # symmetry enforced as they are all muscle excitations. Their behaviour will be + # contstrained by the periodicity imposed on their respective activations. + symmetryGoal.addControlPair(osim.MocoPeriodicityGoalPair('/lumbarAct')) + + + # Effort: + # Get a reference to the MocoControlGoal that is added to a MocoTrack + # problem by default and adjust the weight + effort = osim.MocoControlGoal.safeDownCast(problem.updGoal("control_effort")) + effort.setWeight(10.0) + + + # Ground contact: + # Track the left and right vertical and fore-aft GRFs + contact_r = ["contactHeel_r", "contactFront_r"] + contact_l = ["contactHeel_l", "contactFront_l"] + grf_tracking_weight = 1 + if grf_tracking_weight != 0: + + # Create a contact tracking goal + contactTracking = osim.MocoContactTrackingGoal("contact", grf_tracking_weight) + contactTracking.setExternalLoadsFile("referenceGRF.xml") + + # Add contact groups. The sum of all the contact forces in each group + # should track the force data from a single ExternalForce + contactTracking.addContactGroup(contact_r, "Right_GRF") + contactTracking.addContactGroup(contact_l, "Left_GRF") + + # 2D walking problem so consider force errors in the sagittal plane only + contactTracking.setProjection("plane") + contactTracking.setProjectionVector(osim.Vec3(0, 0, 1)) + + # add the goal to the MocoProblem + problem.addGoal(contactTracking) + + + + # ********************************** + # SET BOUNDS + + # Coordinate bounds as dict + coord_bounds = {} + coord_bounds["/jointset/groundPelvis/pelvis_tilt/value"] = [-20*pi/180, -10*pi/180] + coord_bounds["/jointset/groundPelvis/pelvis_tx/value"] = [0.0, 1.0] + coord_bounds["/jointset/groundPelvis/pelvis_ty/value"] = [0.75, 1.25] + coord_bounds["/jointset/hip_r/hip_flexion_r/value"] = [-10*pi/180, 60*pi/180] + coord_bounds["/jointset/hip_l/hip_flexion_l/value"] = [-10*pi/180, 60*pi/180] + coord_bounds["/jointset/knee_r/knee_angle_r/value"] = [-50*pi/180, 0] + coord_bounds["/jointset/knee_l/knee_angle_l/value"] = [-50*pi/180, 0] + coord_bounds["/jointset/ankle_r/ankle_angle_r/value"] = [-15*pi/180, 25*pi/180] + coord_bounds["/jointset/ankle_l/ankle_angle_l/value"] = [-15*pi/180, 25*pi/180] + coord_bounds["/jointset/lumbar/lumbar/value"] = [0, 20*pi/180] + + # Set coordinate bounds + for bnd in coord_bounds: + problem.setStateInfo(bnd, coord_bounds[bnd]); + + + + # ********************************** + # SOLVE + + # The Solver is pre-configured, however, uncomment below to configure further + # if desired. This gets a writable reference to the Solver, for custom + # configuration. + # solver = study.updSolver() + # solver.set_num_mesh_intervals(50); + # solver.set_verbosity(2); + # solver.set_optim_solver("ipopt"); + # solver.set_optim_convergence_tolerance(1e-4); + # solver.set_optim_constraint_tolerance(1e-4); + # solver.set_optim_max_iterations(1000); + + + # Solve the problem for a single step + solution = study.solve() + + + # Create two mirrored steps from the single step solution, see API + # documentation for use of this utility function + two_steps_solution = osim.createPeriodicTrajectory(solution) + two_steps_solution.write("walk_2D_two_steps_tracking_solution.sto") + + # Also extract the predicted ground forces, see API documentation for use of + # this utility function + contact_forces_table = osim.createExternalLoadsTableForGait(model, two_steps_solution, contact_r, contact_l) + osim.STOFileAdapter().write(contact_forces_table, "walk_2D_two_steps_tracking_ground_forces.sto") + + + return study, solution, two_steps_solution + + + + +""" +gait_prediction(tracking_solution): +Set up a gait prediction problem where the goal is to minimize effort +(squared controls) divided by distance traveled while enforcing symmetry of +the walking cycle and a prescribed average gait speed through endpoint +constraints. The solution of the coordinate tracking problem is used as an +initial guess for the prediction. +""" +def gait_prediction(tracking_solution): + + + + # ********************************** + # DEFINE THE OPTIMAL CONTROL PROBLEM + + # Create predcition problem + study = osim.MocoStudy() + study.setName("gait_prediciton") + + # Get a writable reference to the MocoProblem from the MocoStudy to + # customise the problem settings + problem = study.updProblem() + + # Construct a ModelProcessor and set it on the Problem + modelProcessor = osim.ModelProcessor("2D_gait.osim") + problem.setModelProcessor(modelProcessor) + + + + # ********************************** + # SET GOALS + + # Symmetry: This goal allows us to simulate only one step with left-right + # symmetry that we can then double to create two steps, one on each leg + # (IFO>IFS>CFO>CFS). Note that this is not actually a full gait cycle + # (IFO>IFS>CFO>CFS>IFO) + symmetryGoal = osim.MocoPeriodicityGoal("symmetry_goal") + problem.addGoal(symmetryGoal) + + + # Enforce periodic symmetry + model = modelProcessor.process() + model.initSystem() + state_names = [model.getStateVariableNames().getitem(sn) for sn in range(model.getNumStateVariables())] + for sn in state_names: + + # Symmetric coordinate values and speeds (except for pelvis_tx value): + # Here we constrain final coordinate values of one leg to match the initial + # value of the other leg. Manually add pelvis_tx speed later. + if "jointset" in sn: + if "_r" in sn: + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, sn.replace("_r", "_l"))) + elif "_l" in sn: + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, sn.replace("_l", "_r"))) + elif "pelvis_tx" not in sn: + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn)) + + # Symmetric muscle activations: + # Here, we constrain final muscle activation values of one leg to match the + # initial activation values of the other leg. + elif "activation" in sn: + if "_r" in sn: + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, sn.replace("_r", "_l"))) + elif "_l" in sn: + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, sn.replace("_l", "_r"))) + + # The pelvis_tx speed has periodic symmetry + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair("/jointset/groundPelvis/pelvis_tx/speed")) + + # Lumbar control has periodic symmetry. The other controls don't need + # symmetry enforces as they are all muscle excitations. Their behaviour will be + # modulated by the periodicity imposed on their respective activations. + symmetryGoal.addControlPair(osim.MocoPeriodicityGoalPair('/lumbarAct')) + + # Specify the desired average walk speed, add as a goal + speedGoal = osim.MocoAverageSpeedGoal('avg_speed') + speedGoal.set_desired_average_speed(1.2) + problem.addGoal(speedGoal) + + # Minimise total "effort" over the distance, i.e. minimise sum of the absolute + # values of all controls raised to a given exponent, integrated over the phase. + # In a MocoTrack problem, this is automatically added, however, in this + # predictive problem, we have created a MocoStudy from scratch, so we need to + # add this goal manually to the Problem. The second input argument to the + # constructor is the weight applied to this goal. We also normalise the effort + # by the distance travelled. + effortGoal = osim.MocoControlGoal('effort', 10) + effortGoal.setExponent(3) + effortGoal.setDivideByDisplacement(True) + problem.addGoal(effortGoal) + + + + # ********************************** + # SET BOUNDS + + # set time bounds + problem.setTimeBounds(0, [0.4, 0.6]) + + + # Coordinate bounds as dict + coord_bounds = {} + coord_bounds["/jointset/groundPelvis/pelvis_tilt/value"] = [-20*pi/180, -10*pi/180] + coord_bounds["/jointset/groundPelvis/pelvis_tx/value"] = [0.0, 1.0] + coord_bounds["/jointset/groundPelvis/pelvis_ty/value"] = [0.75, 1.25] + coord_bounds["/jointset/hip_r/hip_flexion_r/value"] = [-10*pi/180, 60*pi/180] + coord_bounds["/jointset/hip_l/hip_flexion_l/value"] = [-10*pi/180, 60*pi/180] + coord_bounds["/jointset/knee_r/knee_angle_r/value"] = [-50*pi/180, 0] + coord_bounds["/jointset/knee_l/knee_angle_l/value"] = [-50*pi/180, 0] + coord_bounds["/jointset/ankle_r/ankle_angle_r/value"] = [-15*pi/180, 25*pi/180] + coord_bounds["/jointset/ankle_l/ankle_angle_l/value"] = [-15*pi/180, 25*pi/180] + coord_bounds["/jointset/lumbar/lumbar/value"] = [0, 20*pi/180] + + # Set coordinate bounds + for bnd in coord_bounds: + problem.setStateInfo(bnd, coord_bounds[bnd]) + + + + # ********************************** + # SOLVE + + # Configure the solver. In the tracking problem, the solver was pre-configured, + # using MocoTrack, however, as we have created a MocoStudy from scratch, we + # need to initialise the MocoSolver and configure it. + solver = study.initCasADiSolver() + solver.set_num_mesh_intervals(50) + solver.set_verbosity(2) + solver.set_optim_solver("ipopt") + solver.set_optim_convergence_tolerance(1e-4) + solver.set_optim_constraint_tolerance(1e-4) + solver.set_optim_max_iterations(1000) + + # Set the tracking solution for one step as the initial guess + solver.setGuess(tracking_solution) + + + # Solve the problem for a single step + solution = study.solve() + + + # Create two mirrored steps from the single step solution, see API + # documentation for use of this utility function + two_steps_solution = osim.createPeriodicTrajectory(solution) + two_steps_solution.write("walk_2D_two_steps_prediction_solution.sto") + + # Also extract the predicted ground forces, see API documentation for use of + # this utility function + contact_r = ["contactHeel_r", "contactFront_r"] + contact_l = ["contactHeel_l", "contactFront_l"] + contact_forces_table = osim.createExternalLoadsTableForGait(model, two_steps_solution, contact_r, contact_l) + osim.STOFileAdapter().write(contact_forces_table, "walk_2D_two_steps_prediction_ground_forces.sto") + + + return study, solution, two_steps_solution + + + +# %% TRACKING PROBLEM + +# Solve the problem and visualise +tracking_study, tracking_solution, tracking_two_steps_solution = gait_tracking() +tracking_study.visualize(tracking_two_steps_solution) + + + +# %% PREDICTION PROBLEM + +# Solve the problem and visualise (uses tracking_solution as initial guess) +prediction_study, prediction_solution, prediction_two_steps_solution = gait_prediction(tracking_solution) +prediction_study.visualize(prediction_two_steps_solution) + + diff --git a/Bindings/Python/examples/Moco/example2DWalking/example2DWalkingMetabolics.py b/Bindings/Python/examples/Moco/example2DWalking/example2DWalkingMetabolics.py new file mode 100644 index 0000000000..13c6fad8d6 --- /dev/null +++ b/Bindings/Python/examples/Moco/example2DWalking/example2DWalkingMetabolics.py @@ -0,0 +1,206 @@ +# -*- coding: utf-8 -*- +""" +MOCO: WALKING 2D EXAMPLE - COORDINATE TRACKING, MINIMISE METABOLIC COST + +@author: Prasanna Sritharan, June 2022 + +This is a Python implementation of an example optimal control +problem (2D walking) orginally created in C++ by Antoine Falisse +(see: example2DWalkingMetabolics.cpp) and adapted for Matlab by Brian Umberger +(see: example2DWalkingMetabolics.m). + +This example features a tracking simulation of walking that includes +minimisation of the metabolic cost of transport computed using a smooth +approximation of the metabolic energy model of Bhargava et al (2004). + +Set a coordinate tracking problem where the goal is to minimize the +difference between provided and simulated coordinate values and speeds +as well as to minimize an effort cost (squared controls) and a metabolic +cost (metabolic energy normalized by distance traveled and body mass; +the metabolics model is based on a smooth approximation of the +phenomenological model described by Bhargava et al. (2004)). The provided +data represents half a gait cycle. Endpoint constraints enforce periodicity +of the coordinate values (except for pelvis tx) and speeds, coordinate +actuator controls, and muscle activations. +""" + + +from math import pi +import opensim as osim + + + +# %% SET THE MODEL AND METABOLICS ANALYSIS + +# Get the OpenSim model +model = osim.Model("2D_gait.osim") + +# Add a component to calculate metabolics +metabolics = osim.Bhargava2004SmoothedMuscleMetabolics() +metabolics.setName("metabolics") +metabolics.set_use_smoothing(True) + +# Add muscles to metabolics component +leg = ["r", "l"] +musclist = ["hamstrings", "bifemsh", "glut_max", "iliopsoas", "rect_fem", + "vasti", "gastroc", "soleus", "tib_ant"] +for mc in [m + "_" + l for m in musclist for l in leg]: + metabolics.addMuscle(mc, osim.Muscle.safeDownCast(model.getComponent(mc))) + +# Add metabolics component to model +model.addComponent(metabolics) +model.finalizeConnections() + + + +# %% DEFINE THE OPTIMAL CONTROL PROBLEM + +# Create the tracking problem +track = osim.MocoTrack() +track.setName("gait_tracking_met_cost") + +# Pass the model to MocoTrack +modelProcessor = osim.ModelProcessor(model) +track.setModel(modelProcessor) + +# Reference data for coordinate tracking +tableProcessor = osim.TableProcessor("referenceCoordinates.sto") +tableProcessor.append(osim.TabOpLowPassFilter(6.0)) +track.setStatesReference(tableProcessor) + +# Remaining MocoTrack settings +track.set_states_global_tracking_weight(30) +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(0.0) +track.set_final_time(0.47008941) + +# Call initialize() to get the internal MocoStudy. The MocoStudy is comprised +# of a MocoProblem and a MocoSolver. Get the MocoProblem to configure further. +study = track.initialize() +problem = study.updProblem() + + + +# %% SET GOALS + +# Symmetry: +# This goal allows us to simulate only one step with left-right symmetry that +# we can then double to create two steps, one on each leg (IFO>IFS>CFO>CFS). +# Note that this is not actually a full gait cycle (IFO>IFS>CFO>CFS>IFO). +symmetryGoal = osim.MocoPeriodicityGoal("symmetry_goal") +problem.addGoal(symmetryGoal) + +# Enforce periodic symmetry +model = modelProcessor.process() +model.initSystem() +state_names = [model.getStateVariableNames().getitem(sn) for sn in range(model.getNumStateVariables())] +for sn in state_names: + + # Symmetric coordinate values and speeds (except for pelvis_tx value): + # Here we constrain final coordinate values of one leg to match the initial + # value of the other leg. Manually add pelvis_tx speed later. + if "jointset" in sn: + if "_r" in sn: + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, sn.replace("_r", "_l"))) + elif "_l" in sn: + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, sn.replace("_l", "_r"))) + elif "pelvis_tx" not in sn: + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn)) + + # Symmetric muscle activations: + # Here, we constrain final muscle activation values of one leg to match the + # initial activation values of the other leg. + elif "activation" in sn: + if "_r" in sn: + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, sn.replace("_r", "_l"))) + elif "_l" in sn: + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, sn.replace("_l", "_r"))) + + +# The pelvis_tx speed has periodic symmetry +symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair("/jointset/groundPelvis/pelvis_tx/speed")) + +# Lumbar control has periodic symmetry. The other controls don't need symmetry +# enforced as they are all muscle excitations. Their behaviour will be +# modulated by the periodicity imposed on their respective activations. +symmetryGoal.addControlPair(osim.MocoPeriodicityGoalPair('/lumbarAct')) + + +# Effort: +# Get a reference to the MocoControlGoal that is added to a MocoTrack +# problem by default and adjust the weight +effort = osim.MocoControlGoal().safeDownCast(problem.updGoal("control_effort")) +effort.setWeight(0.1) + + +# Metabolic cost: +# Total metabolic rate includes activation heat rate, maintenance heat rate, +# shortening heat rate, mechanical work rate, and basal metabolic rate. +met_weight = 0.1 +metabolicsGoal = osim.MocoOutputGoal("metabolics", met_weight) +metabolicsGoal.setOutputPath("/metabolics|total_metabolic_rate") +metabolicsGoal.setDivideByDisplacement(True) +metabolicsGoal.setDivideByMass(True) +problem.addGoal(metabolicsGoal) + + + +# %% SET BOUNDS + +# Coordinate bounds as dict +coord_bounds = {} +coord_bounds["/jointset/groundPelvis/pelvis_tilt/value"] = [-20*pi/180, -10*pi/180] +coord_bounds["/jointset/groundPelvis/pelvis_tx/value"] = [0.0, 1.0] +coord_bounds["/jointset/groundPelvis/pelvis_ty/value"] = [0.75, 1.25] +coord_bounds["/jointset/hip_r/hip_flexion_r/value"] = [-10*pi/180, 60*pi/180] +coord_bounds["/jointset/hip_l/hip_flexion_l/value"] = [-10*pi/180, 60*pi/180] +coord_bounds["/jointset/knee_r/knee_angle_r/value"] = [-50*pi/180, 0] +coord_bounds["/jointset/knee_l/knee_angle_l/value"] = [-50*pi/180, 0] +coord_bounds["/jointset/ankle_r/ankle_angle_r/value"] = [-15*pi/180, 25*pi/180] +coord_bounds["/jointset/ankle_l/ankle_angle_l/value"] = [-15*pi/180, 25*pi/180] +coord_bounds["/jointset/lumbar/lumbar/value"] = [0, 20*pi/180] + +# Set coordinate bounds +for bnd in coord_bounds: + problem.setStateInfo(bnd, coord_bounds[bnd]); + + + +# %% SOLVE + +# Configure the solver +solver = study.initCasADiSolver() +solver.set_num_mesh_intervals(50) +solver.set_verbosity(2) +solver.set_optim_solver("ipopt") +solver.set_optim_convergence_tolerance(1e-4) +solver.set_optim_constraint_tolerance(1e-4) +solver.set_optim_max_iterations(10000) + +# Solve the problem for a single step +solution = study.solve() + + +# Create two mirrored steps from the single step solution, see API +# documentation for use of this utility function +two_steps_solution = osim.createPeriodicTrajectory(solution) +two_steps_solution.write("walk_2D_metabolics_two_steps_tracking_solution.sto") + +# Also extract the predicted ground forces, see API documentation for use of +# this utility function +contact_r = ["contactHeel_r", "contactFront_r"] +contact_l = ["contactHeel_l", "contactFront_l"] +contact_forces_table = osim.createExternalLoadsTableForGait(model, two_steps_solution, contact_r, contact_l) +osim.STOFileAdapter().write(contact_forces_table, "walk_2D_metabolics_two_steps_tracking_ground_forces.sto") + + +# Determine the cost of transport from the metabolics cost term. Need to divide +# by the weight applied earlier. +COT = solution.getObjectiveTerm("metabolics") / met_weight +print("\nThe metabolic cost of transport is: %f" % COT) + + +# visualise +study.visualize(two_steps_solution) \ No newline at end of file diff --git a/Bindings/Python/examples/Moco/example2DWalking/example2DWalkingStepAsymmetry.py b/Bindings/Python/examples/Moco/example2DWalking/example2DWalkingStepAsymmetry.py new file mode 100644 index 0000000000..502f51c813 --- /dev/null +++ b/Bindings/Python/examples/Moco/example2DWalking/example2DWalkingStepAsymmetry.py @@ -0,0 +1,511 @@ +# -*- coding: utf-8 -*- +""" +MOCO: WALKING 2D EXAMPLE - STEP ASYMMETRY + +@author: Prasanna Sritharan, June 2022 + + +This is a Python implementation of an example optimal control problem +originally written by Russell T. Johnson for Matlab +(see example2DWalkingStepAsymmetry.m). + +Simulate asymmetric gait using MocoStepTimeAsymmetryGoal and +MocoStepLengthAsymmetryGoal. This is an extension of the example2DWalking +MATLAB example (see example2DWalking.m for details about model and data used). +""" + + +from math import pi +import opensim as osim +import os +import numpy as np + + + +""" +stepTimeAsymmetry(): + +Set up a predictive optimization problem where the goal is to minimize an +effort cost (cubed controls) and hit a target step time asymmetry. Unlike +example2DWalking, this problem requires simulating a full gait cycle. +Additionally, endpoint constraints enforce periodicity of the coordinate values +(except for pelvis tx) and speeds, coordinate actuator controls, and muscle +activations. + +Step time is defined as the time between consecutive foot strikes. Step Time +Asymmetry (STA) is a ratio and is calculated as follows: + - Right Step Time (RST) = Time from left foot-strike to right foot-strike + - Left Step Time (LST) = Time from right foot-strike to left foot-strike + - STA = (RST - LST) / (RST + LST) + +The step time goal works by "counting" the number of nodes that each foot is in +contact with the ground (with respect to a specified contact force threshold). +Since, in walking, there are double support phases where both feet are on the +ground, the goal also detects which foot is in front and assigns the step time +to the leading foot. Altogether, it estimates the time between consecutive +heel strikes in order to infer the left and right step times. + +The contact elements for each foot must specified via 'setLeftContactGroup()' +and 'setRightContactGroup()'. The force element and force threshold used to +determine when a foot is in contact is set via 'setContactForceDirection()' and +'setContactForceThreshold()'. + +Users must provide the target asymmetry value via 'setTargetAsymmetry()'. +Asymmetry values ranges from -1.0 to 1.0. For example, 0.20 is 20positive +step time asymmetry with greater right step times than left step times. A +symmetric step times solution can be achieved by setting this property to zero. +This goal can be used only in 'cost' mode, where the error between the target +asymmetry and model asymmetry is squared. To make this goal suitable for +gradient-based optimization, step time values are assigned via smoothing +functions which can be controlled via 'setAsymmetrySmoothing()' and +'setContactDetectionSmoothing()'. + +Since this goal doesn't directly compute the step time asymmetry from heel +strikes, users should confirm that the step time asymmetry from the solution +matches closely to the target. To do this, we provide the helper function +computeStepAsymmetryValues() below. +""" +def stepTimeAsymmetry(): + + + # ********************************** + # DEFINE THE OPTIMAL CONTROL PROBLEM + + # Create a MocoStudy + study = osim.MocoStudy() + study.setName("step_time_asymmetry") + + # Get the model + modelProcessor = osim.ModelProcessor("2D_gait.osim") + modelProcessor.append(osim.ModOpTendonComplianceDynamicsModeDGF("implicit")) + + # Get the MocoProblem from the MocoStudy and set the model on it + problem = study.updProblem() + problem.setModelProcessor(modelProcessor) + + + + # ********************************** + # SET GOALS + + # Periodicity: + # All states are periodic except pelvis_tx value, lumbar actuator control + # is periodic. + periodicityGoal = osim.MocoPeriodicityGoal("periodicity") + model = modelProcessor.process() + model.initSystem() + state_names = [model.getStateVariableNames().getitem(sn) for sn in range(model.getNumStateVariables())] + for sn in state_names: + if "pelvis_tx/value" not in sn: + periodicityGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn)) + periodicityGoal.addControlPair(osim.MocoPeriodicityGoalPair("/lumbarAct")) + problem.addGoal(periodicityGoal) + + # Average gait speed + speedGoal = osim.MocoAverageSpeedGoal("avg_speed") + speedGoal.set_desired_average_speed(1.0) + problem.addGoal(speedGoal) + + # Effort + effortGoal = osim.MocoControlGoal("effort", 10.0) + effortGoal.setExponent(3) + effortGoal.setDivideByDisplacement(True) + problem.addGoal(effortGoal) + + # Step time asymmetry: + # Create the goal, and configure it + stepTimeAsymmetryGoal = osim.MocoStepTimeAsymmetryGoal("step_time_asymmetry") + # Value for smoothing term used to compute when foot contact is made + # (default = 0.25). Users may need to adjust this based on convergence and + # matching the target asymmetry. + stepTimeAsymmetryGoal.setContactDetectionSmoothing(0.4) + # Contact threshold based on vertical GRF (default = 25 N) + stepTimeAsymmetryGoal.setContactForceThreshold(25.0) + # Value for smoothing term used to compute asymmetry (default = 10). Users + # may need to adjust this based on convergence and matching the target + # asymmetry. + stepTimeAsymmetryGoal.setAsymmetrySmoothing(3.0) + # Target step length asymmetry. Positive values mean greater right step + # length than left. + stepTimeAsymmetryGoal.setTargetAsymmetry(0.10) + # Set goal weight + stepTimeAsymmetryGoal.setWeight(5.0) + # Need to define the names of the left and right heel spheres: this is + # used to detect which foot is in front during double support phase. + contact_r = ["contactHeel_r", "contactFront_r"] + contact_l = ["contactHeel_l", "contactFront_l"] + stepTimeAsymmetryGoal.setRightContactGroup(contact_r, "contactHeel_r") + stepTimeAsymmetryGoal.setLeftContactGroup(contact_l, "contactHeel_l") + # Add the goal to the problem + problem.addGoal(stepTimeAsymmetryGoal) + + + + # ********************************** + # SET BOUNDS + + # Set time bounds + problem.setTimeBounds(0.0, 0.94) + + # Coordinate bounds as dict + coord_bounds = {} + coord_bounds["/jointset/groundPelvis/pelvis_tilt/value"] = [-20*pi/180, 20*pi/180] + coord_bounds["/jointset/groundPelvis/pelvis_tx/value"] = [0, 2] + coord_bounds["/jointset/groundPelvis/pelvis_ty/value"] = [0.75, 1.25] + coord_bounds["/jointset/hip_r/hip_flexion_r/value"] = [-10*pi/180, 60*pi/180] + coord_bounds["/jointset/hip_l/hip_flexion_l/value"] = [-10*pi/180, 60*pi/180] + coord_bounds["/jointset/knee_r/knee_angle_r/value"] = [-50*pi/180, 0] + coord_bounds["/jointset/knee_l/knee_angle_l/value"] = [-50*pi/180, 0] + coord_bounds["/jointset/ankle_r/ankle_angle_r/value"] = [-15*pi/180, 25*pi/180] + coord_bounds["/jointset/ankle_l/ankle_angle_l/value"] = [-15*pi/180, 25*pi/180] + coord_bounds["/jointset/lumbar/lumbar/value"] = [0, 20*pi/180] + + # Set coordinate bounds + for bnd in coord_bounds: + problem.setStateInfo(bnd, coord_bounds[bnd]) + + + + # ********************************** + # SOLVE + + # Configure the solver + solver = study.initCasADiSolver() + solver.set_num_mesh_intervals(100) + solver.set_verbosity(2) + solver.set_optim_convergence_tolerance(1e-4) + solver.set_optim_constraint_tolerance(1e-4) + solver.set_optim_max_iterations(2000) + + # Set the initial guess to be the symmetric two-steps tracking solution + # from example2DWalking.py. Run this first, or proceed without a guess. + guess_file = "walk_2D_two_steps_tracking_solution.sto" + if os.path.isfile(guess_file): + solver.setGuessFile(guess_file) + + # Print the Study to file + study.printToXML("example2DWalkingStepTimeAsymmetry.omoco") + + + # Solve + solution = study.solve() + solution.write("walk_2D_step_time_asym_solution.sto") + + + # Write predicted GRF to file + contact_forces_table = osim.createExternalLoadsTableForGait(model, solution, contact_r, contact_l) + osim.STOFileAdapter().write(contact_forces_table, "walk_2D_step_time_asym_ground_forces.sto") + + # Calculate step time asymmetry + step_time_asym, _ = computeStepAsymmetry(model, 25, "walk_2D_step_time_asym_solution.sto", "walk_2D_step_time_asym_ground_forces.sto") + print("\nStep time asymmetry: %f\n" % step_time_asym) + + + return study, solution + + + +""" +stepLengthAsymmetry(): + +This goal works by limiting the distance between feet, or "foot frames", +throughout the gait cycle. The goal calculates the distance between the left +foot and right foot, then limits the distance between feet to not pass beyond +minimum (negative) or maximum (positive) bounds. There are two limits used: +one that limits the distance between feet when the right foot is in front, and +one that limits the distance between feet when the left foot is in front. + +Step Length Asymmetry (SLA) is a ratio and is calculated as follows: +The Right Step Length (RSL) is the distance between feet at right foot strike +The Left Step Length (LSL) is the distance between feet at left foot strike +Step Length Asymmetry = (RSL - LSL)/ (RSL + LSL) + +Users must provide the target asymmetry value via 'setTargetAsymmetry()'. +Asymmetry values ranges from -1.0 to 1.0. For example, 0.20 is 20positive +step length asymmetry with greater right step length than left step length. A +symmetric step length solution can be achieved by setting this property to zero. +This goal can be used only in 'cost' mode, where the error between the target +asymmetry and model asymmetry is squared. To make this goal suitable for +gradient-based optimization, step length values are assigned via a smoothing +function which can be controlled via 'setAsymmetrySmoothing()'. + +Users must also prescribed the stride length via 'setStrideLength()'. The goal +then calculates the minimum and maximum bounds on the distance between right +and left foot. Users must ensure that this stride length is met via problem +bounds or other goals; the value provided to MocoStepLengthAsymmetryGoal is +only used to compute the model's asymmetry in the cost function. + +Because this goal doesn't directly compute the step length asymmetry from +heel strike data, users should confirm that the step length asymmetry +from the solution matches closely to their target. To do this, we +provide the helper function computeStepAsymmetryValues() below. Users may +also want to confirm that the stride length from the optimization +matches with setStrideLength(), or set additional constraints for stride length +within the optimization. Additionally, in some cases users may want to set +target asymmetries above or below the desired value, in the event there is +some offset. +""" +def stepLengthAsymmetry(): + + + # ********************************** + # DEFINE THE OPTIMAL CONTROL PROBLEM + + # Create a MocoStudy + study = osim.MocoStudy() + study.setName("step_length_asymmetry") + + # Get the model + modelProcessor = osim.ModelProcessor("2D_gait.osim") + modelProcessor.append(osim.ModOpTendonComplianceDynamicsModeDGF("implicit")) + + # Get the MocoProblem from the MocoStudy and set the model on it + problem = study.updProblem() + problem.setModelProcessor(modelProcessor) + + + + # ********************************** + # SET GOALS + + # Periodicity: + # All states are periodic except pelvis_tx value, lumbar actuator control + # is periodic. + periodicityGoal = osim.MocoPeriodicityGoal("periodicity") + model = modelProcessor.process() + model.initSystem() + state_names = [model.getStateVariableNames().getitem(sn) for sn in range(model.getNumStateVariables())] + for sn in state_names: + if "pelvis_tx/value" not in sn: + periodicityGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn)) + periodicityGoal.addControlPair(osim.MocoPeriodicityGoalPair("/lumbarAct")) + problem.addGoal(periodicityGoal) + + # Average gait speed + speedGoal = osim.MocoAverageSpeedGoal("avg_speed") + speedGoal.set_desired_average_speed(1.0) + problem.addGoal(speedGoal) + + # Effort + effortGoal = osim.MocoControlGoal("effort", 10.0) + effortGoal.setExponent(3) + effortGoal.setDivideByDisplacement(True) + problem.addGoal(effortGoal) + + # Step length asymmetry: + # Create the goal, and configure it + stepLengthAsymmetryGoal = osim.MocoStepLengthAsymmetryGoal() + # Set body names for left and right foot + stepLengthAsymmetryGoal.setRightFootFrame('/bodyset/calcn_r') + stepLengthAsymmetryGoal.setLeftFootFrame('/bodyset/calcn_l') + # Provide the stride length for the simulation (m) + stepLengthAsymmetryGoal.setStrideLength(0.904) + # Value for smoothing term used to compute asymmetry (default = 5). Users + # may need to adjust this based on convergence and matching the target + # asymmetry. + stepLengthAsymmetryGoal.setAsymmetrySmoothing(5) + # Target step length asymmetry. Positive values mean greater right step + # length than left. + stepLengthAsymmetryGoal.setTargetAsymmetry(-0.10) + # Set goal weight + stepLengthAsymmetryGoal.setWeight(5) + # Add the goal to the problem + problem.addGoal(stepLengthAsymmetryGoal) + + + + # ********************************** + # SET BOUNDS + + # Set time bounds + problem.setTimeBounds(0.0, 0.94) + + # Coordinate bounds as dict + coord_bounds = {} + coord_bounds["/jointset/groundPelvis/pelvis_tilt/value"] = [-20*pi/180, 20*pi/180] + coord_bounds["/jointset/groundPelvis/pelvis_tx/value"] = [0, 2] + coord_bounds["/jointset/groundPelvis/pelvis_ty/value"] = [0.75, 1.25] + coord_bounds["/jointset/hip_r/hip_flexion_r/value"] = [-10*pi/180, 60*pi/180] + coord_bounds["/jointset/hip_l/hip_flexion_l/value"] = [-10*pi/180, 60*pi/180] + coord_bounds["/jointset/knee_r/knee_angle_r/value"] = [-50*pi/180, 0] + coord_bounds["/jointset/knee_l/knee_angle_l/value"] = [-50*pi/180, 0] + coord_bounds["/jointset/ankle_r/ankle_angle_r/value"] = [-15*pi/180, 25*pi/180] + coord_bounds["/jointset/ankle_l/ankle_angle_l/value"] = [-15*pi/180, 25*pi/180] + coord_bounds["/jointset/lumbar/lumbar/value"] = [0, 20*pi/180] + + # Set coordinate bounds + for bnd in coord_bounds: + problem.setStateInfo(bnd, coord_bounds[bnd]) + + + + # ********************************** + # SOLVE + + # Configure the solver + solver = study.initCasADiSolver() + solver.set_num_mesh_intervals(100) + solver.set_verbosity(2) + solver.set_optim_convergence_tolerance(1e-4) + solver.set_optim_constraint_tolerance(1e-4) + solver.set_optim_max_iterations(2000) + + # Set the initial guess to be the symmetric two-steps tracking solution + # from example2DWalking.py. Run this first, or proceed without a guess. + guess_file = "walk_2D_two_steps_tracking_solution.sto" + if os.path.isfile(guess_file): + solver.setGuessFile(guess_file) + + # Print the Study to file + study.printToXML("example2DWalkingStepLengthAsymmetry.omoco") + + + # Solve + solution = study.solve() + solution.write("walk_2D_step_length_asym_solution.sto") + + # Write predicted GRF to file + contact_r = ["contactHeel_r", "contactFront_r"] + contact_l = ["contactHeel_l", "contactFront_l"] + contact_forces_table = osim.createExternalLoadsTableForGait(model, solution, contact_r, contact_l) + osim.STOFileAdapter().write(contact_forces_table, "walk_2D_step_length_asym_ground_forces.sto") + + # Calculate step time asymmetry + _, step_length_asym = computeStepAsymmetry(model, 25, "walk_2D_step_length_asym_solution.sto", "walk_2D_step_length_asym_ground_forces.sto") + print("\nStep length asymmetry: %f\n" % step_length_asym) + + + return study, solution + + + +""" +computeStepAsymmetry(): + +Calculate the values of the step length and step time asymmetry from the +results of the simulation. +""" +def computeStepAsymmetry(model_file, threshold, solution_file, grf_file): + + # Load model + model = osim.Model(model_file) + + # Load predicted GRF + grf = np.genfromtxt(grf_file, skip_header=5, delimiter="\t") + + # GRF time vector and vertical components + tvec = grf[:, 0] + + + # ********************************** + # STEP TIME ASYMMETRY + + # Find index of heel strike on each leg + hs_idxR = findHeelStrikeIndex(grf[:, 2], threshold) + hs_idxL = findHeelStrikeIndex(grf[:, 8], threshold) + + # Compute step time on each leg + hs_timeR = tvec[hs_idxR] + hs_timeL = tvec[hs_idxL] + if hs_timeR < hs_timeL: + step_timeL = hs_timeL - hs_timeR + step_timeR = tvec[-1] - step_timeL + else: + step_timeR = hs_timeR - hs_timeL + step_timeL = tvec[-1] - step_timeR + + # Calculate step time asymmetry (%) + step_time_asym = 100 * (step_timeR - step_timeL) / (step_timeR + step_timeL) + + + # ********************************** + # STEP LENGTH ASYMMETRY + + # Get the states for each limb at the instant of heel strike on that limb + states_traj = osim.StatesTrajectory().createFromStatesTable(model, osim.TimeSeriesTable(solution_file), False, True, True) + statesR = states_traj.get(hs_idxR) + statesL = states_traj.get(hs_idxL) + + # Calculate the step length + step_lengthR = calculateStepLength(model, statesR) + step_lengthL = calculateStepLength(model, statesL) + + # Calculate step length asymmetry (%) + step_length_asym = 100 * (step_lengthR - step_lengthL) / (step_lengthR + step_lengthL) + + + return step_time_asym, step_length_asym + + + +""" +findHeelStrikeIndex(): + +Find heel strike index by determining foot contact on-off instances. If no +heel strike is found, then assume first index is heel strike. This +implementation differs from that of Russell T. Johnson's Matlab version, but +follows the same prinicples. +""" +def findHeelStrikeIndex(grfy, threshold): + + # Find windows representing ground contact + is_contact = (grfy > threshold).astype(int) + + # Calculate transition points, i.e. heel strike (1) and foot off (-1) + contact_diff = np.diff(np.insert(is_contact, 0, 1)) + + # Extract heel strike and foot off indices. If no heel strike found, + # assume first index is heel strike. + idxs = np.where(contact_diff == 1)[0] + if idxs.size == 0: + idx = 0 + else: + idx = idxs[0] + + return int(idx) + + + +""" +calculateStepLength(): + +Find step length by configuring the model at heel strike, then compute distance +between contact spheres along the fore-aft coordinate. +""" +def calculateStepLength(model, state): + + # Configure the model at heel strike + model.initSystem() + model.realizePosition(state) + + # Get the heel contact spheres + contact_r = model.getContactGeometrySet().get("heel_r") + contact_l = model.getContactGeometrySet().get("heel_l") + + # Find the positions of the contact spheres in the global frame + pos_r = contact_r.getFrame().getPositionInGround(state) + pos_l = contact_l.getFrame().getPositionInGround(state) + + # Step length is the difference between global position of the left and + # right along the fore-aft coordinate (x) + step_length = abs(pos_r.get(0) - pos_l.get(0)) + + return step_length + + + + +# %% STEP TIME ASYMMETRY + +# Solve and visualise +step_time_study, step_time_solution = stepTimeAsymmetry() +step_time_study.visualize(step_time_solution) + + + +# %% STEP LENGTH ASYMMETRY + +# Solve and visualise +step_length_study, step_length_solution = stepLengthAsymmetry() +step_length_study.visualize(step_length_solution) + diff --git a/Bindings/Python/examples/Moco/example2DWalking/referenceCoordinates.sto b/Bindings/Python/examples/Moco/example2DWalking/referenceCoordinates.sto new file mode 100644 index 0000000000..49c1856b97 --- /dev/null +++ b/Bindings/Python/examples/Moco/example2DWalking/referenceCoordinates.sto @@ -0,0 +1,56 @@ +version=1 +nRows=50 +nColumns=11 +inDegrees=no +endheader +time /jointset/groundPelvis/pelvis_tilt/value /jointset/groundPelvis/pelvis_tx/value /jointset/groundPelvis/pelvis_ty/value /jointset/hip_l/hip_flexion_l/value /jointset/hip_r/hip_flexion_r/value /jointset/knee_l/knee_angle_l/value /jointset/knee_r/knee_angle_r/value /jointset/ankle_l/ankle_angle_l/value /jointset/ankle_r/ankle_angle_r/value /jointset/lumbar/lumbar/value +0 -0.263901172 0 0.88365423 0.016469227 0.664226832 -0.283287738 -0.184426289 0.100897108 0.241505399 0.167890235 +0.00959366 -0.263237135 0.01477811 0.88286864 0.014834571 0.655766195 -0.309360488 -0.173860159 0.079944899 0.23368018 0.169581415 +0.01918732 -0.262380255 0.02981966 0.88203224 0.014574244 0.645644188 -0.338356542 -0.168285571 0.057179922 0.224945653 0.17150017 +0.02878098 -0.261375157 0.04510758 0.88116841 0.015814559 0.632672942 -0.370221258 -0.169806187 0.03225246 0.215265722 0.173715833 +0.03837465 -0.260365147 0.06045601 0.88040404 0.018636448 0.617605696 -0.404630306 -0.17528638 0.006652231 0.2053268 0.176063851 +0.04796831 -0.259541769 0.07552426 0.87996855 0.023508629 0.602627326 -0.439970753 -0.180172488 -0.015752649 0.195721202 0.178138544 +0.05756197 -0.259054514 0.0900652 0.88012353 0.030924383 0.588387845 -0.473795415 -0.184675993 -0.033251158 0.186446945 0.179697019 +0.06715563 -0.258944308 0.10401582 0.88102469 0.040812304 0.574930106 -0.505010842 -0.188707257 -0.04635638 0.177759897 0.180719341 +0.07674929 -0.259129324 0.11745937 0.88263129 0.052879604 0.561930784 -0.533978387 -0.192057021 -0.055232943 0.169944453 0.181312727 +0.08634295 -0.259494498 0.13052005 0.88473361 0.066767753 0.549021373 -0.561206172 -0.1945697 -0.060103311 0.163201244 0.181599832 +0.09593662 -0.259946181 0.14330536 0.88709913 0.082205144 0.535908621 -0.587001137 -0.196126516 -0.061239178 0.15763983 0.181673436 +0.10553028 -0.260424344 0.15588956 0.88954517 0.098981772 0.522427673 -0.61151102 -0.196694092 -0.058902809 0.153288535 0.181589336 +0.11512394 -0.260895925 0.16831964 0.89194813 0.116940376 0.508499514 -0.634758933 -0.196280109 -0.053378743 0.150129505 0.181378224 +0.1247176 -0.261344146 0.18062565 0.89422878 0.135951798 0.494097143 -0.656690572 -0.194919508 -0.044959219 0.148116785 0.18105773 +0.13431126 -0.261760474 0.19282791 0.89633673 0.155908169 0.47922707 -0.677220781 -0.19267299 -0.033938675 0.147185144 0.180638595 +0.14390492 -0.262140251 0.20494149 0.89823955 0.176712591 0.463918059 -0.696243373 -0.189625148 -0.020612934 0.147255397 0.180128698 +0.15349858 -0.262480488 0.2169788 0.89991627 0.19827165 0.448215384 -0.713640048 -0.185881233 -0.005281646 0.14823791 0.179535128 +0.16309225 -0.262779029 0.22895074 0.90135424 0.220490754 0.432176747 -0.729285592 -0.181563646 0.011751635 0.150035399 0.178865135 +0.17268591 -0.26303437 0.24086683 0.90254857 0.243275884 0.415871218 -0.743047478 -0.176811974 0.030180266 0.152546468 0.178125628 +0.18227957 -0.26324622 0.25273466 0.90350253 0.266539966 0.39937649 -0.754783066 -0.171763051 0.049695868 0.15566883 0.177322116 +0.19187323 -0.263416705 0.26456153 0.90422669 0.290178829 0.382760737 -0.764361195 -0.166540245 0.06998758 0.159289606 0.176462865 +0.20146689 -0.263548689 0.27635373 0.90473797 0.314090464 0.366096416 -0.771648066 -0.16127501 0.09074392 0.16327494 0.175555437 +0.21106055 -0.263644631 0.28811681 0.90505705 0.338178841 0.34946335 -0.776507563 -0.156104717 0.111653878 0.167483997 0.174605815 +0.22065421 -0.263707062 0.29985674 0.90520533 0.362331265 0.332945267 -0.778818108 -0.151175098 0.132406904 0.171777842 0.173620572 +0.23024788 -0.263739786 0.31158062 0.90520302 0.386426416 0.31659922 -0.778481885 -0.146585483 0.152694553 0.176027951 0.172609028 +0.23984154 -0.2637483 0.3232968 0.90506819 0.410344037 0.300439585 -0.775428078 -0.142358927 0.172213726 0.180117981 0.171584122 +0.2494352 -0.263737527 0.33501406 0.90481637 0.43394679 0.284490683 -0.769621757 -0.138543468 0.190672344 0.18392919 0.170558684 +0.25902886 -0.263711718 0.34674065 0.90446067 0.457095181 0.268790075 -0.761047023 -0.135202579 0.207800883 0.187354915 0.1695436 +0.26862252 -0.263675066 0.3584856 0.90401181 0.479636001 0.253365432 -0.74975634 -0.13238925 0.223365577 0.190313693 0.168551632 +0.27821618 -0.263632059 0.37025742 0.90347895 0.501429082 0.238233036 -0.735829337 -0.130139148 0.237187888 0.192753458 0.167594908 +0.28780985 -0.263587296 0.38206273 0.90287023 0.522366351 0.223407778 -0.719342179 -0.128470889 0.249161679 0.194654099 0.166681952 +0.29740351 -0.263544928 0.39390784 0.90219222 0.542348415 0.208902072 -0.700411802 -0.127413669 0.259259119 0.196027907 0.165820444 +0.30699717 -0.263508812 0.40579709 0.90145035 0.561323972 0.194721571 -0.679134063 -0.126983291 0.26752961 0.196923886 0.16501368 +0.31659083 -0.263482642 0.41773411 0.90064844 0.579268997 0.18086429 -0.655596358 -0.127186852 0.274082766 0.19741978 0.164262844 +0.32618449 -0.263470241 0.42972222 0.89978852 0.59617356 0.167325862 -0.629892637 -0.128024382 0.279064674 0.197608562 0.163567518 +0.33577815 -0.263476545 0.44176595 0.89887075 0.612011344 0.154095665 -0.602153429 -0.129486079 0.282634911 0.197582259 0.162929015 +0.34537181 -0.263508411 0.45387176 0.89789403 0.626721369 0.141155521 -0.572512693 -0.131553545 0.284952621 0.197403826 0.162352733 +0.35496548 -0.263571485 0.46604776 0.89685722 0.640223615 0.128499339 -0.541170932 -0.13423171 0.286171047 0.197087781 0.161845392 +0.36455914 -0.263667009 0.47830591 0.89576296 0.65240092 0.116142625 -0.508586093 -0.137584219 0.286432272 0.196606361 0.161417274 +0.3741528 -0.263795524 0.49066444 0.89462361 0.663026895 0.104120714 -0.475355592 -0.141719617 0.285863414 0.195854526 0.161091135 +0.38374646 -0.26395744 0.50314483 0.89346085 0.67182581 0.092488606 -0.442041485 -0.146762118 0.284576033 0.19461586 0.160897007 +0.39334012 -0.264147207 0.51576584 0.89229721 0.678633505 0.081323148 -0.409129284 -0.152859379 0.282664158 0.192606219 0.160855576 +0.40293378 -0.264350906 0.52854205 0.89115067 0.683448148 0.070716099 -0.377000866 -0.16018322 0.280199755 0.189537207 0.160973895 +0.41252745 -0.264547355 0.54148631 0.89003554 0.686389878 0.060759752 -0.346033277 -0.168913938 0.277228166 0.185159684 0.161250717 +0.42212111 -0.264715253 0.5546125 0.888964 0.687584644 0.05153945 -0.316484219 -0.179220643 0.273770226 0.17924927 0.161687636 +0.43171477 -0.264834637 0.56793507 0.88794522 0.68715166 0.043132573 -0.288559446 -0.191252879 0.269824084 0.171593841 0.162288809 +0.44130843 -0.26488361 0.58146821 0.88698448 0.685223599 0.03561966 -0.262556525 -0.205167641 0.265367335 0.162003974 0.163056655 +0.45090209 -0.264840126 0.59522634 0.88608284 0.68188844 0.029097011 -0.238729181 -0.221144437 0.260364772 0.150288055 0.163996272 +0.46049575 -0.264679681 0.60922304 0.88523602 0.677229457 0.02366775 -0.217423937 -0.239373949 0.254766118 0.136263805 0.165112063 +0.47008941 -0.264375087 0.62347006 0.88443285 0.671340242 0.019430621 -0.199143374 -0.260034354 0.248505043 0.119800092 0.166406632 diff --git a/Bindings/Python/examples/Moco/example2DWalking/referenceGRF.sto b/Bindings/Python/examples/Moco/example2DWalking/referenceGRF.sto new file mode 100644 index 0000000000..ea7372e1bc --- /dev/null +++ b/Bindings/Python/examples/Moco/example2DWalking/referenceGRF.sto @@ -0,0 +1,56 @@ +version=1 +nRows=50 +nColumns=19 +inDegrees=no +endheader +time ground_force_r_vx ground_force_r_vy ground_force_r_vz ground_force_l_vx ground_force_l_vy ground_force_l_vz ground_force_r_px ground_force_r_py ground_force_r_pz ground_torque_r_x ground_torque_r_y ground_torque_r_z ground_force_l_px ground_force_l_py ground_force_l_pz ground_torque_l_x ground_torque_l_y ground_torque_l_z +0 -44.72876818 29.04783005 0 116.9150482 518.9919061 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.00959366 -80.70993994 58.17685056 0 112.5300264 447.22788 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.01918732 -194.1883161 163.4317854 0 96.94474098 369.4335826 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.02878098 -101.1143896 473.9459685 0 71.19198143 269.1178404 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.03837465 -123.1892148 832.2568261 0 20.52447173 107.1657532 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.04796831 -162.6262962 1082.657734 0 -0.351146207 0.399165328 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.05756197 -162.3223543 1137.739521 0 0.735020556 -0.697238179 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.06715563 -133.0618758 1020.885259 0 0.063943811 -0.053070133 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.07674929 -94.31653198 849.0253536 0 0.008493774 -0.006365101 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.08634295 -63.94628978 703.0735561 0 0.005012414 -0.003452482 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.09593662 -43.36830109 602.3660585 0 0.017215667 -0.011031164 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.10553028 -29.98021554 538.4096336 0 0.003994266 -0.002401198 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.11512394 -20.86032418 498.9031722 0 -0.103988334 0.059018415 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.1247176 -14.15437725 474.5977654 0 -0.07149884 0.038510149 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.13431126 -8.682453945 459.6932278 0 -0.043882155 0.022527374 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.14390492 -3.767695979 450.99912 0 -0.030029999 0.014749761 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.15349858 0.5711294 447.3545664 0 -0.024522938 0.011563929 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.16309225 4.627454508 448.9596287 0 -0.024375213 0.011071356 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.17268591 8.52901387 455.9589672 0 -0.029440536 0.012916646 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.18227957 9.836865735 467.6678075 0 -0.042527454 0.018066634 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.19187323 13.2070312 482.7432469 0 -0.070878936 0.029253636 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.20146689 15.14708192 498.9268941 0 -0.132125005 0.053077912 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.21106055 17.81920746 514.8192008 0 -0.263673162 0.103364878 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.22065421 18.78544929 529.1738275 0 -0.539813205 0.206966148 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.23024788 20.75158299 541.8117521 0 -1.082144951 0.406899831 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.23984154 21.26780054 552.4245249 0 -2.041623387 0.754504993 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.2494352 24.11255405 561.0728088 0 -3.480735071 1.268525498 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.25902886 23.01025613 568.1294535 0 -5.231812724 1.884036085 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.26862252 25.24384531 574.1443889 0 -6.73844898 2.408260617 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.27821618 24.24124078 579.2951272 0 -7.385977339 2.623760693 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.28780985 23.34982403 583.8783841 0 -6.773099693 2.39836048 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.29740351 22.31168622 587.8150344 0 -5.081857993 1.796612744 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.30699717 21.16301834 590.8194335 0 -2.986235199 1.05487801 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.31659083 20.79062274 592.5106855 0 -1.279853528 0.451987769 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.32618449 19.3131051 592.7073041 0 -0.400529529 0.14155292 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.33577815 18.83286351 591.3300124 0 -0.503920494 0.178513154 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.34537181 19.3077819 589.3487921 0 -2.660445586 0.943528286 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.35496548 21.04548114 588.8376909 0 -9.676101293 3.448528052 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.36455914 25.66171516 591.3196451 0 -22.07264702 7.967110626 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.3741528 32.92462926 596.1476995 0 -33.61999039 12.41666689 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.38374646 40.98291968 600.4637207 0 -38.79529227 14.80407071 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.39334012 50.03052978 604.7580299 0 -38.45857743 15.26104242 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.40293378 59.06582568 609.712179 0 -35.30109901 14.61504568 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.41252745 68.31852304 613.8200683 0 -31.25816585 13.5369722 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.42212111 77.82744972 616.1591504 0 -27.65935088 12.54038274 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.43171477 87.50658842 615.5098877 0 -25.00832044 11.89319162 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.44130843 97.27797645 610.7621406 0 -23.65794881 11.83094037 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.45090209 106.6857723 601.3876317 0 -23.96903654 12.64370381 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.46049575 115.2949437 585.3046159 0 -26.4721059 14.81458097 0 0 0 0 0 0 0 0 0 0 0 0 0 +0.47008941 119.1204798 559.9827889 0 -32.28560944 19.3260534 0 0 0 0 0 0 0 0 0 0 0 0 0 diff --git a/Bindings/Python/examples/Moco/example2DWalking/referenceGRF.xml b/Bindings/Python/examples/Moco/example2DWalking/referenceGRF.xml new file mode 100644 index 0000000000..f16e4519a9 --- /dev/null +++ b/Bindings/Python/examples/Moco/example2DWalking/referenceGRF.xml @@ -0,0 +1,32 @@ + + + + + + + calcn_r + + ground + + ground + + ground_force_r_v + + ground_force_r_p + + ground_torque_r_ + + + calcn_l + ground + ground + ground_force_l_v + ground_force_l_p + ground_torque_l_ + + + + + referenceGRF.sto + + From 6692f47d3f4a1439c7ae317429e6a8900ac9a8ea Mon Sep 17 00:00:00 2001 From: psbiomech Date: Mon, 27 Jun 2022 15:08:17 +1000 Subject: [PATCH 3/8] Delete .gitkeep --- Bindings/Python/examples/Moco/example2DWalking/.gitkeep | 1 - 1 file changed, 1 deletion(-) delete mode 100644 Bindings/Python/examples/Moco/example2DWalking/.gitkeep diff --git a/Bindings/Python/examples/Moco/example2DWalking/.gitkeep b/Bindings/Python/examples/Moco/example2DWalking/.gitkeep deleted file mode 100644 index 8b13789179..0000000000 --- a/Bindings/Python/examples/Moco/example2DWalking/.gitkeep +++ /dev/null @@ -1 +0,0 @@ - From eca870d435f1ba37e1e60e2c1aae5f9ad451c8f0 Mon Sep 17 00:00:00 2001 From: psbiomech Date: Mon, 27 Jun 2022 15:52:07 +1000 Subject: [PATCH 4/8] Updated to 80 chars per ine --- Bindings/Python/examples/example2DWalking.py | 416 ++++++++++++++ .../examples/example2DWalkingMetabolics.py | 219 +++++++ .../examples/example2DWalkingStepAsymmetry.py | 537 ++++++++++++++++++ 3 files changed, 1172 insertions(+) create mode 100644 Bindings/Python/examples/example2DWalking.py create mode 100644 Bindings/Python/examples/example2DWalkingMetabolics.py create mode 100644 Bindings/Python/examples/example2DWalkingStepAsymmetry.py diff --git a/Bindings/Python/examples/example2DWalking.py b/Bindings/Python/examples/example2DWalking.py new file mode 100644 index 0000000000..4a788cd379 --- /dev/null +++ b/Bindings/Python/examples/example2DWalking.py @@ -0,0 +1,416 @@ +# -*- coding: utf-8 -*- +""" +MOCO: WALKING 2D EXAMPLE - TRACKING & PREDICTION + +@author: Prasanna Sritharan, June 2022 + +This is a Python implementation of an example optimal control +problem (2D walking) orginally created in C++ by Antoine Falisse +(see: example2DWalking.cpp) and adapted for Matlab by Brian Umberger +(see: example2DWalking.m). +""" + +from math import pi +import opensim as osim + + + + +""" +gait_tracking(): +Set up a coordinate tracking problem where the goal is to minimize the +difference between provided and simulated coordinate values and speeds (and +ground reaction forces), as well as to minimize an effort cost (squared +controls). The provided data represents one step. Endpoint constraints +enforce periodicity of the coordinate values (except for pelvis tx) and speeds, +coordinate actuator controls, and muscle activations. +""" +def gait_tracking(): + + + + # ********************************** + # DEFINE THE OPTIMAL CONTROL PROBLEM + + # Create tracking problem + track = osim.MocoTrack() + track.setName("gait_tracking") + + # Construct a ModelProcessor and set it on the tool + modelProcessor = osim.ModelProcessor("2D_gait.osim") + track.setModel(modelProcessor) + + + # Get the coordinates into a TableProcessor + tableProcessor = osim.TableProcessor("referenceCoordinates.sto") + tableProcessor.append(osim.TabOpLowPassFilter(6.0)) + + # Set the coordinates as the reference states + track.setStatesReference(tableProcessor) + track.set_allow_unused_references(True) + + # Only coordinates are provided so derive to get speeds and track these too + track.set_track_reference_position_derivatives(True) + + # use coordinates and speeds for initial guess + track.set_apply_tracked_states_to_guess(True) + + # Define the global state tracking weight + track.set_states_global_tracking_weight(1.0) + + + # Set initial time, final time and mesh interval + track.set_initial_time(0.0) + track.set_final_time(0.47008941) + + # Call initialize() to receive a pre-configured MocoStudy object based on + # the settings above. Use this to customize the problem beyond the + # Mocotrack interface. + study = track.initialize() + + # Get a writable reference to the MocoProblem from the MocoStudy to perform + # more customisation + problem = study.updProblem() + + + + # ********************************** + # SET GOALS + + # Symmetry: + # This goal allows us to simulate only one step with left-right symmetry + # that we can then double to create two steps, one on each leg + # (IFO>IFS>CFO>CFS). Note that this is not actually a full gait cycle + # (IFO>IFS>CFO>CFS>IFO). + symmetryGoal = osim.MocoPeriodicityGoal("symmetry_goal") + problem.addGoal(symmetryGoal) + + # Enforce periodic symmetry + model = modelProcessor.process() + model.initSystem() + state_names = [model.getStateVariableNames().getitem(sn) + for sn in range(model.getNumStateVariables())] + for sn in state_names: + + # Symmetric coordinate values and speeds (except for pelvis_tx value): + # Here we constrain final coordinate values of one leg to match the + # initial value of the other leg. Manually add pelvis_tx speed later. + if "jointset" in sn: + if "_r" in sn: + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, + sn.replace("_r", "_l"))) + elif "_l" in sn: + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, + sn.replace("_l", "_r"))) + elif "pelvis_tx" not in sn: + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn)) + + # Symmetric muscle activations: + # Here, we constrain final muscle activation values of one leg to match + # the initial activation values of the other leg. + elif "activation" in sn: + if "_r" in sn: + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, + sn.replace("_r", "_l"))) + elif "_l" in sn: + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, + sn.replace("_l", "_r"))) + + + # The pelvis_tx speed has periodic symmetry + symmetryGoal.addStatePair( + osim.MocoPeriodicityGoalPair("/jointset/groundPelvis/pelvis_tx/speed")) + + # Lumbar control has periodic symmetry. The other controls don't need + # symmetry enforced as they are all muscle excitations. Their behaviour + # will be contstrained by the periodicity imposed on their respective + # activations. + symmetryGoal.addControlPair(osim.MocoPeriodicityGoalPair('/lumbarAct')) + + + # Effort: + # Get a reference to the MocoControlGoal that is added to a MocoTrack + # problem by default and adjust the weight + effort = osim.MocoControlGoal.safeDownCast( + problem.updGoal("control_effort")) + effort.setWeight(10.0) + + + # Ground contact: + # Track the left and right vertical and fore-aft GRFs + contact_r = ["contactHeel_r", "contactFront_r"] + contact_l = ["contactHeel_l", "contactFront_l"] + grf_tracking_weight = 1 + if grf_tracking_weight != 0: + + # Create a contact tracking goal + contactTracking = osim.MocoContactTrackingGoal("contact", + grf_tracking_weight) + contactTracking.setExternalLoadsFile("referenceGRF.xml") + + # Add contact groups. The sum of all the contact forces in each group + # should track the force data from a single ExternalForce + contactTracking.addContactGroup(contact_r, "Right_GRF") + contactTracking.addContactGroup(contact_l, "Left_GRF") + + # 2D walking problem so consider force errors in the sagittal plane + contactTracking.setProjection("plane") + contactTracking.setProjectionVector(osim.Vec3(0, 0, 1)) + + # add the goal to the MocoProblem + problem.addGoal(contactTracking) + + + + # ********************************** + # SET BOUNDS + + # Coordinate bounds as dict + coord_bounds = {} + coord_bounds["/jointset/groundPelvis/pelvis_tilt/value"] = [-20*pi/180, + -10*pi/180] + coord_bounds["/jointset/groundPelvis/pelvis_tx/value"] = [0.0, 1.0] + coord_bounds["/jointset/groundPelvis/pelvis_ty/value"] = [0.75, 1.25] + coord_bounds["/jointset/hip_r/hip_flexion_r/value"] = [-10*pi/180, + 60*pi/180] + coord_bounds["/jointset/hip_l/hip_flexion_l/value"] = [-10*pi/180, + 60*pi/180] + coord_bounds["/jointset/knee_r/knee_angle_r/value"] = [-50*pi/180, 0] + coord_bounds["/jointset/knee_l/knee_angle_l/value"] = [-50*pi/180, 0] + coord_bounds["/jointset/ankle_r/ankle_angle_r/value"] = [-15*pi/180, + 25*pi/180] + coord_bounds["/jointset/ankle_l/ankle_angle_l/value"] = [-15*pi/180, + 25*pi/180] + coord_bounds["/jointset/lumbar/lumbar/value"] = [0, 20*pi/180] + + # Set coordinate bounds + for bnd in coord_bounds: + problem.setStateInfo(bnd, coord_bounds[bnd]); + + + + # ********************************** + # SOLVE + + # The Solver is pre-configured, however, uncomment below to configure + # further if desired. This gets a writable reference to the Solver, for + # custom configuration. + # solver = study.updSolver() + # solver.set_num_mesh_intervals(50); + # solver.set_verbosity(2); + # solver.set_optim_solver("ipopt"); + # solver.set_optim_convergence_tolerance(1e-4); + # solver.set_optim_constraint_tolerance(1e-4); + # solver.set_optim_max_iterations(1000); + + + # Solve the problem for a single step + solution = study.solve() + + + # Create two mirrored steps from the single step solution, see API + # documentation for use of this utility function + two_steps_solution = osim.createPeriodicTrajectory(solution) + two_steps_solution.write("walk_2D_two_steps_tracking_solution.sto") + + # Also extract the predicted ground forces, see API documentation for use + # of this utility function + contact_forces_table = osim.createExternalLoadsTableForGait(model, + two_steps_solution, contact_r, contact_l) + osim.STOFileAdapter().write(contact_forces_table, + "walk_2D_two_steps_tracking_ground_forces.sto") + + + return study, solution, two_steps_solution + + + + +""" +gait_prediction(tracking_solution): +Set up a gait prediction problem where the goal is to minimize effort +(squared controls) divided by distance traveled while enforcing symmetry of +the walking cycle and a prescribed average gait speed through endpoint +constraints. The solution of the coordinate tracking problem is used as an +initial guess for the prediction. +""" +def gait_prediction(tracking_solution): + + + + # ********************************** + # DEFINE THE OPTIMAL CONTROL PROBLEM + + # Create predcition problem + study = osim.MocoStudy() + study.setName("gait_prediciton") + + # Get a writable reference to the MocoProblem from the MocoStudy to + # customise the problem settings + problem = study.updProblem() + + # Construct a ModelProcessor and set it on the Problem + modelProcessor = osim.ModelProcessor("2D_gait.osim") + problem.setModelProcessor(modelProcessor) + + + + # ********************************** + # SET GOALS + + # Symmetry: This goal allows us to simulate only one step with left-right + # symmetry that we can then double to create two steps, one on each leg + # (IFO>IFS>CFO>CFS). Note that this is not actually a full gait cycle + # (IFO>IFS>CFO>CFS>IFO) + symmetryGoal = osim.MocoPeriodicityGoal("symmetry_goal") + problem.addGoal(symmetryGoal) + + + # Enforce periodic symmetry + model = modelProcessor.process() + model.initSystem() + state_names = [model.getStateVariableNames().getitem(sn) + for sn in range(model.getNumStateVariables())] + for sn in state_names: + + # Symmetric coordinate values and speeds (except for pelvis_tx value): + # Here we constrain final coordinate values of one leg to match the + # initial value of the other leg. Manually add pelvis_tx speed later. + if "jointset" in sn: + if "_r" in sn: + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, + sn.replace("_r", "_l"))) + elif "_l" in sn: + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, + sn.replace("_l", "_r"))) + elif "pelvis_tx" not in sn: + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn)) + + # Symmetric muscle activations: + # Here, we constrain final muscle activation values of one leg to match + # the initial activation values of the other leg. + elif "activation" in sn: + if "_r" in sn: + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, + sn.replace("_r", "_l"))) + elif "_l" in sn: + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, + sn.replace("_l", "_r"))) + + # The pelvis_tx speed has periodic symmetry + symmetryGoal.addStatePair( + osim.MocoPeriodicityGoalPair("/jointset/groundPelvis/pelvis_tx/speed")) + + # Lumbar control has periodic symmetry. The other controls don't need + # symmetry enforces as they are all muscle excitations. Their behaviour + # will be modulated by the periodicity imposed on their activations. + symmetryGoal.addControlPair(osim.MocoPeriodicityGoalPair('/lumbarAct')) + + # Specify the desired average walk speed, add as a goal + speedGoal = osim.MocoAverageSpeedGoal('avg_speed') + speedGoal.set_desired_average_speed(1.2) + problem.addGoal(speedGoal) + + # Minimise total "effort" over the distance, i.e. minimise sum of the + # absolute values of all controls raised to a given exponent, integrated + # over the phase. In a MocoTrack problem, this is automatically added, + # however, in this predictive problem, we have created a MocoStudy from + # scratch, so we need to add this goal manually to the Problem. The second + # input argument to the constructor is the weight applied to this goal. + # We also normalise the effort by the distance travelled. + effortGoal = osim.MocoControlGoal('effort', 10) + effortGoal.setExponent(3) + effortGoal.setDivideByDisplacement(True) + problem.addGoal(effortGoal) + + + + # ********************************** + # SET BOUNDS + + # set time bounds + problem.setTimeBounds(0, [0.4, 0.6]) + + + # Coordinate bounds as dict + coord_bounds = {} + coord_bounds["/jointset/groundPelvis/pelvis_tilt/value"] = [-20*pi/180, + -10*pi/180] + coord_bounds["/jointset/groundPelvis/pelvis_tx/value"] = [0.0, 1.0] + coord_bounds["/jointset/groundPelvis/pelvis_ty/value"] = [0.75, 1.25] + coord_bounds["/jointset/hip_r/hip_flexion_r/value"] = [-10*pi/180, + 60*pi/180] + coord_bounds["/jointset/hip_l/hip_flexion_l/value"] = [-10*pi/180, + 60*pi/180] + coord_bounds["/jointset/knee_r/knee_angle_r/value"] = [-50*pi/180, 0] + coord_bounds["/jointset/knee_l/knee_angle_l/value"] = [-50*pi/180, 0] + coord_bounds["/jointset/ankle_r/ankle_angle_r/value"] = [-15*pi/180, + 25*pi/180] + coord_bounds["/jointset/ankle_l/ankle_angle_l/value"] = [-15*pi/180, + 25*pi/180] + coord_bounds["/jointset/lumbar/lumbar/value"] = [0, 20*pi/180] + + # Set coordinate bounds + for bnd in coord_bounds: + problem.setStateInfo(bnd, coord_bounds[bnd]) + + + + # ********************************** + # SOLVE + + # Configure the solver. In the tracking problem, the solver was + # pre-configured, using MocoTrack, however, as we have created a MocoStudy + # from scratch, we need to initialise the MocoSolver and configure it. + solver = study.initCasADiSolver() + solver.set_num_mesh_intervals(50) + solver.set_verbosity(2) + solver.set_optim_solver("ipopt") + solver.set_optim_convergence_tolerance(1e-4) + solver.set_optim_constraint_tolerance(1e-4) + solver.set_optim_max_iterations(1000) + + # Set the tracking solution for one step as the initial guess + solver.setGuess(tracking_solution) + + + # Solve the problem for a single step + solution = study.solve() + + + # Create two mirrored steps from the single step solution, see API + # documentation for use of this utility function + two_steps_solution = osim.createPeriodicTrajectory(solution) + two_steps_solution.write("walk_2D_two_steps_prediction_solution.sto") + + # Also extract the predicted ground forces, see API documentation for use + # of this utility function + contact_r = ["contactHeel_r", "contactFront_r"] + contact_l = ["contactHeel_l", "contactFront_l"] + contact_forces_table = osim.createExternalLoadsTableForGait(model, + two_steps_solution, contact_r, contact_l) + osim.STOFileAdapter().write(contact_forces_table, + "walk_2D_two_steps_prediction_ground_forces.sto") + + + return study, solution, two_steps_solution + + + +# %% TRACKING PROBLEM + +# Solve the problem and visualise +tracking_study, tracking_solution, tracking_two_steps_solution = \ + gait_tracking() +tracking_study.visualize(tracking_two_steps_solution) + + + +# %% PREDICTION PROBLEM + +# Solve the problem and visualise (uses tracking_solution as initial guess) +prediction_study, prediction_solution, prediction_two_steps_solution = \ + gait_prediction(tracking_solution) +prediction_study.visualize(prediction_two_steps_solution) + + diff --git a/Bindings/Python/examples/example2DWalkingMetabolics.py b/Bindings/Python/examples/example2DWalkingMetabolics.py new file mode 100644 index 0000000000..88b36600cf --- /dev/null +++ b/Bindings/Python/examples/example2DWalkingMetabolics.py @@ -0,0 +1,219 @@ +# -*- coding: utf-8 -*- +""" +MOCO: WALKING 2D EXAMPLE - COORDINATE TRACKING, MINIMISE METABOLIC COST + +@author: Prasanna Sritharan, June 2022 + +This is a Python implementation of an example optimal control +problem (2D walking) orginally created in C++ by Antoine Falisse +(see: example2DWalkingMetabolics.cpp) and adapted for Matlab by Brian Umberger +(see: example2DWalkingMetabolics.m). + +This example features a tracking simulation of walking that includes +minimisation of the metabolic cost of transport computed using a smooth +approximation of the metabolic energy model of Bhargava et al (2004). + +Set a coordinate tracking problem where the goal is to minimize the +difference between provided and simulated coordinate values and speeds +as well as to minimize an effort cost (squared controls) and a metabolic +cost (metabolic energy normalized by distance traveled and body mass; +the metabolics model is based on a smooth approximation of the +phenomenological model described by Bhargava et al. (2004)). The provided +data represents half a gait cycle. Endpoint constraints enforce periodicity +of the coordinate values (except for pelvis tx) and speeds, coordinate +actuator controls, and muscle activations. +""" + + +from math import pi +import opensim as osim + + + +# %% SET THE MODEL AND METABOLICS ANALYSIS + +# Get the OpenSim model +model = osim.Model("2D_gait.osim") + +# Add a component to calculate metabolics +metabolics = osim.Bhargava2004SmoothedMuscleMetabolics() +metabolics.setName("metabolics") +metabolics.set_use_smoothing(True) + +# Add muscles to metabolics component +leg = ["r", "l"] +musclist = ["hamstrings", "bifemsh", "glut_max", "iliopsoas", "rect_fem", + "vasti", "gastroc", "soleus", "tib_ant"] +for mc in [m + "_" + l for m in musclist for l in leg]: + metabolics.addMuscle(mc, osim.Muscle.safeDownCast(model.getComponent(mc))) + +# Add metabolics component to model +model.addComponent(metabolics) +model.finalizeConnections() + + + +# %% DEFINE THE OPTIMAL CONTROL PROBLEM + +# Create the tracking problem +track = osim.MocoTrack() +track.setName("gait_tracking_met_cost") + +# Pass the model to MocoTrack +modelProcessor = osim.ModelProcessor(model) +track.setModel(modelProcessor) + +# Reference data for coordinate tracking +tableProcessor = osim.TableProcessor("referenceCoordinates.sto") +tableProcessor.append(osim.TabOpLowPassFilter(6.0)) +track.setStatesReference(tableProcessor) + +# Remaining MocoTrack settings +track.set_states_global_tracking_weight(30) +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(0.0) +track.set_final_time(0.47008941) + +# Call initialize() to get the internal MocoStudy. The MocoStudy is comprised +# of a MocoProblem and a MocoSolver. Get the MocoProblem to configure further. +study = track.initialize() +problem = study.updProblem() + + + +# %% SET GOALS + +# Symmetry: +# This goal allows us to simulate only one step with left-right symmetry that +# we can then double to create two steps, one on each leg (IFO>IFS>CFO>CFS). +# Note that this is not actually a full gait cycle (IFO>IFS>CFO>CFS>IFO). +symmetryGoal = osim.MocoPeriodicityGoal("symmetry_goal") +problem.addGoal(symmetryGoal) + +# Enforce periodic symmetry +model = modelProcessor.process() +model.initSystem() +state_names = [model.getStateVariableNames().getitem(sn) + for sn in range(model.getNumStateVariables())] +for sn in state_names: + + # Symmetric coordinate values and speeds (except for pelvis_tx value): + # Here we constrain final coordinate values of one leg to match the initial + # value of the other leg. Manually add pelvis_tx speed later. + if "jointset" in sn: + if "_r" in sn: + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, + sn.replace("_r", "_l"))) + elif "_l" in sn: + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, + sn.replace("_l", "_r"))) + elif "pelvis_tx" not in sn: + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn)) + + # Symmetric muscle activations: + # Here, we constrain final muscle activation values of one leg to match the + # initial activation values of the other leg. + elif "activation" in sn: + if "_r" in sn: + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, + sn.replace("_r", "_l"))) + elif "_l" in sn: + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, + sn.replace("_l", "_r"))) + + +# The pelvis_tx speed has periodic symmetry +symmetryGoal.addStatePair( + osim.MocoPeriodicityGoalPair("/jointset/groundPelvis/pelvis_tx/speed")) + +# Lumbar control has periodic symmetry. The other controls don't need symmetry +# enforced as they are all muscle excitations. Their behaviour will be +# modulated by the periodicity imposed on their respective activations. +symmetryGoal.addControlPair(osim.MocoPeriodicityGoalPair('/lumbarAct')) + + +# Effort: +# Get a reference to the MocoControlGoal that is added to a MocoTrack +# problem by default and adjust the weight +effort = osim.MocoControlGoal().safeDownCast(problem.updGoal("control_effort")) +effort.setWeight(0.1) + + +# Metabolic cost: +# Total metabolic rate includes activation heat rate, maintenance heat rate, +# shortening heat rate, mechanical work rate, and basal metabolic rate. +met_weight = 0.1 +metabolicsGoal = osim.MocoOutputGoal("metabolics", met_weight) +metabolicsGoal.setOutputPath("/metabolics|total_metabolic_rate") +metabolicsGoal.setDivideByDisplacement(True) +metabolicsGoal.setDivideByMass(True) +problem.addGoal(metabolicsGoal) + + + +# %% SET BOUNDS + +# Coordinate bounds as dict +coord_bounds = {} +coord_bounds["/jointset/groundPelvis/pelvis_tilt/value"] = [-20*pi/180, + -10*pi/180] +coord_bounds["/jointset/groundPelvis/pelvis_tx/value"] = [0.0, 1.0] +coord_bounds["/jointset/groundPelvis/pelvis_ty/value"] = [0.75, 1.25] +coord_bounds["/jointset/hip_r/hip_flexion_r/value"] = [-10*pi/180, + 60*pi/180] +coord_bounds["/jointset/hip_l/hip_flexion_l/value"] = [-10*pi/180, + 60*pi/180] +coord_bounds["/jointset/knee_r/knee_angle_r/value"] = [-50*pi/180, 0] +coord_bounds["/jointset/knee_l/knee_angle_l/value"] = [-50*pi/180, 0] +coord_bounds["/jointset/ankle_r/ankle_angle_r/value"] = [-15*pi/180, + 25*pi/180] +coord_bounds["/jointset/ankle_l/ankle_angle_l/value"] = [-15*pi/180, + 25*pi/180] +coord_bounds["/jointset/lumbar/lumbar/value"] = [0, 20*pi/180] + +# Set coordinate bounds +for bnd in coord_bounds: + problem.setStateInfo(bnd, coord_bounds[bnd]); + + + +# %% SOLVE + +# Configure the solver +solver = study.initCasADiSolver() +solver.set_num_mesh_intervals(50) +solver.set_verbosity(2) +solver.set_optim_solver("ipopt") +solver.set_optim_convergence_tolerance(1e-4) +solver.set_optim_constraint_tolerance(1e-4) +solver.set_optim_max_iterations(10000) + +# Solve the problem for a single step +solution = study.solve() + + +# Create two mirrored steps from the single step solution, see API +# documentation for use of this utility function +two_steps_solution = osim.createPeriodicTrajectory(solution) +two_steps_solution.write("walk_2D_metabolics_two_steps_tracking_solution.sto") + +# Also extract the predicted ground forces, see API documentation for use of +# this utility function +contact_r = ["contactHeel_r", "contactFront_r"] +contact_l = ["contactHeel_l", "contactFront_l"] +contact_forces_table = osim.createExternalLoadsTableForGait(model, + two_steps_solution, contact_r, contact_l) +osim.STOFileAdapter().write(contact_forces_table, + "walk_2D_metabolics_two_steps_tracking_ground_forces.sto") + + +# Determine the cost of transport from the metabolics cost term. Need to divide +# by the weight applied earlier. +COT = solution.getObjectiveTerm("metabolics") / met_weight +print("\nThe metabolic cost of transport is: %f" % COT) + + +# visualise +study.visualize(two_steps_solution) \ No newline at end of file diff --git a/Bindings/Python/examples/example2DWalkingStepAsymmetry.py b/Bindings/Python/examples/example2DWalkingStepAsymmetry.py new file mode 100644 index 0000000000..fd8fdfdefe --- /dev/null +++ b/Bindings/Python/examples/example2DWalkingStepAsymmetry.py @@ -0,0 +1,537 @@ +# -*- coding: utf-8 -*- +""" +MOCO: WALKING 2D EXAMPLE - STEP ASYMMETRY + +@author: Prasanna Sritharan, June 2022 + + +This is a Python implementation of an example optimal control problem +originally written by Russell T. Johnson for Matlab +(see example2DWalkingStepAsymmetry.m). + +Simulate asymmetric gait using MocoStepTimeAsymmetryGoal and +MocoStepLengthAsymmetryGoal. This is an extension of the example2DWalking +MATLAB example (see example2DWalking.m for details about model and data used). +""" + + +from math import pi +import opensim as osim +import os +import numpy as np + + + +""" +stepTimeAsymmetry(): + +Set up a predictive optimization problem where the goal is to minimize an +effort cost (cubed controls) and hit a target step time asymmetry. Unlike +example2DWalking, this problem requires simulating a full gait cycle. +Additionally, endpoint constraints enforce periodicity of the coordinate values +(except for pelvis tx) and speeds, coordinate actuator controls, and muscle +activations. + +Step time is defined as the time between consecutive foot strikes. Step Time +Asymmetry (STA) is a ratio and is calculated as follows: + - Right Step Time (RST) = Time from left foot-strike to right foot-strike + - Left Step Time (LST) = Time from right foot-strike to left foot-strike + - STA = (RST - LST) / (RST + LST) + +The step time goal works by "counting" the number of nodes that each foot is in +contact with the ground (with respect to a specified contact force threshold). +Since, in walking, there are double support phases where both feet are on the +ground, the goal also detects which foot is in front and assigns the step time +to the leading foot. Altogether, it estimates the time between consecutive +heel strikes in order to infer the left and right step times. + +The contact elements for each foot must specified via 'setLeftContactGroup()' +and 'setRightContactGroup()'. The force element and force threshold used to +determine when a foot is in contact is set via 'setContactForceDirection()' and +'setContactForceThreshold()'. + +Users must provide the target asymmetry value via 'setTargetAsymmetry()'. +Asymmetry values ranges from -1.0 to 1.0. For example, 0.20 is 20positive +step time asymmetry with greater right step times than left step times. A +symmetric step times solution can be achieved by setting this property to zero. +This goal can be used only in 'cost' mode, where the error between the target +asymmetry and model asymmetry is squared. To make this goal suitable for +gradient-based optimization, step time values are assigned via smoothing +functions which can be controlled via 'setAsymmetrySmoothing()' and +'setContactDetectionSmoothing()'. + +Since this goal doesn't directly compute the step time asymmetry from heel +strikes, users should confirm that the step time asymmetry from the solution +matches closely to the target. To do this, we provide the helper function +computeStepAsymmetryValues() below. +""" +def stepTimeAsymmetry(): + + + # ********************************** + # DEFINE THE OPTIMAL CONTROL PROBLEM + + # Create a MocoStudy + study = osim.MocoStudy() + study.setName("step_time_asymmetry") + + # Get the model + modelProcessor = osim.ModelProcessor("2D_gait.osim") + modelProcessor.append( + osim.ModOpTendonComplianceDynamicsModeDGF("implicit")) + + # Get the MocoProblem from the MocoStudy and set the model on it + problem = study.updProblem() + problem.setModelProcessor(modelProcessor) + + + + # ********************************** + # SET GOALS + + # Periodicity: + # All states are periodic except pelvis_tx value, lumbar actuator control + # is periodic. + periodicityGoal = osim.MocoPeriodicityGoal("periodicity") + model = modelProcessor.process() + model.initSystem() + state_names = [model.getStateVariableNames().getitem(sn) + for sn in range(model.getNumStateVariables())] + for sn in state_names: + if "pelvis_tx/value" not in sn: + periodicityGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn)) + periodicityGoal.addControlPair(osim.MocoPeriodicityGoalPair("/lumbarAct")) + problem.addGoal(periodicityGoal) + + # Average gait speed + speedGoal = osim.MocoAverageSpeedGoal("avg_speed") + speedGoal.set_desired_average_speed(1.0) + problem.addGoal(speedGoal) + + # Effort + effortGoal = osim.MocoControlGoal("effort", 10.0) + effortGoal.setExponent(3) + effortGoal.setDivideByDisplacement(True) + problem.addGoal(effortGoal) + + # Step time asymmetry: + # Create the goal, and configure it + stepTimeAsymmetryGoal = \ + osim.MocoStepTimeAsymmetryGoal("step_time_asymmetry") + # Value for smoothing term used to compute when foot contact is made + # (default = 0.25). Users may need to adjust this based on convergence and + # matching the target asymmetry. + stepTimeAsymmetryGoal.setContactDetectionSmoothing(0.4) + # Contact threshold based on vertical GRF (default = 25 N) + stepTimeAsymmetryGoal.setContactForceThreshold(25.0) + # Value for smoothing term used to compute asymmetry (default = 10). Users + # may need to adjust this based on convergence and matching the target + # asymmetry. + stepTimeAsymmetryGoal.setAsymmetrySmoothing(3.0) + # Target step length asymmetry. Positive values mean greater right step + # length than left. + stepTimeAsymmetryGoal.setTargetAsymmetry(0.10) + # Set goal weight + stepTimeAsymmetryGoal.setWeight(5.0) + # Need to define the names of the left and right heel spheres: this is + # used to detect which foot is in front during double support phase. + contact_r = ["contactHeel_r", "contactFront_r"] + contact_l = ["contactHeel_l", "contactFront_l"] + stepTimeAsymmetryGoal.setRightContactGroup(contact_r, "contactHeel_r") + stepTimeAsymmetryGoal.setLeftContactGroup(contact_l, "contactHeel_l") + # Add the goal to the problem + problem.addGoal(stepTimeAsymmetryGoal) + + + + # ********************************** + # SET BOUNDS + + # Set time bounds + problem.setTimeBounds(0.0, 0.94) + + # Coordinate bounds as dict + coord_bounds = {} + coord_bounds["/jointset/groundPelvis/pelvis_tilt/value"] = [-20*pi/180, + 20*pi/180] + coord_bounds["/jointset/groundPelvis/pelvis_tx/value"] = [0, 2] + coord_bounds["/jointset/groundPelvis/pelvis_ty/value"] = [0.75, 1.25] + coord_bounds["/jointset/hip_r/hip_flexion_r/value"] = [-10*pi/180, + 60*pi/180] + coord_bounds["/jointset/hip_l/hip_flexion_l/value"] = [-10*pi/180, + 60*pi/180] + coord_bounds["/jointset/knee_r/knee_angle_r/value"] = [-50*pi/180, 0] + coord_bounds["/jointset/knee_l/knee_angle_l/value"] = [-50*pi/180, 0] + coord_bounds["/jointset/ankle_r/ankle_angle_r/value"] = [-15*pi/180, + 25*pi/180] + coord_bounds["/jointset/ankle_l/ankle_angle_l/value"] = [-15*pi/180, + 25*pi/180] + coord_bounds["/jointset/lumbar/lumbar/value"] = [0, 20*pi/180] + + # Set coordinate bounds + for bnd in coord_bounds: + problem.setStateInfo(bnd, coord_bounds[bnd]) + + + + # ********************************** + # SOLVE + + # Configure the solver + solver = study.initCasADiSolver() + solver.set_num_mesh_intervals(100) + solver.set_verbosity(2) + solver.set_optim_convergence_tolerance(1e-4) + solver.set_optim_constraint_tolerance(1e-4) + solver.set_optim_max_iterations(2000) + + # Set the initial guess to be the symmetric two-steps tracking solution + # from example2DWalking.py. Run this first, or proceed without a guess. + guess_file = "walk_2D_two_steps_tracking_solution.sto" + if os.path.isfile(guess_file): + solver.setGuessFile(guess_file) + + # Print the Study to file + study.printToXML("example2DWalkingStepTimeAsymmetry.omoco") + + + # Solve + solution = study.solve() + solution.write("walk_2D_step_time_asym_solution.sto") + + + # Write predicted GRF to file + contact_forces_table = osim.createExternalLoadsTableForGait(model, + solution, contact_r, contact_l) + osim.STOFileAdapter().write(contact_forces_table, + "walk_2D_step_time_asym_ground_forces.sto") + + # Calculate step time asymmetry + step_time_asym, _ = computeStepAsymmetry(model, 25, + "walk_2D_step_time_asym_solution.sto", + "walk_2D_step_time_asym_ground_forces.sto") + print("\nStep time asymmetry: %f\n" % step_time_asym) + + + return study, solution + + + +""" +stepLengthAsymmetry(): + +This goal works by limiting the distance between feet, or "foot frames", +throughout the gait cycle. The goal calculates the distance between the left +foot and right foot, then limits the distance between feet to not pass beyond +minimum (negative) or maximum (positive) bounds. There are two limits used: +one that limits the distance between feet when the right foot is in front, and +one that limits the distance between feet when the left foot is in front. + +Step Length Asymmetry (SLA) is a ratio and is calculated as follows: +The Right Step Length (RSL) is the distance between feet at right foot strike +The Left Step Length (LSL) is the distance between feet at left foot strike +Step Length Asymmetry = (RSL - LSL)/ (RSL + LSL) + +Users must provide the target asymmetry value via 'setTargetAsymmetry()'. +Asymmetry values ranges from -1.0 to 1.0. For example, 0.20 is 20positive +step length asymmetry with greater right step length than left step length. A +symmetric step length solution can be achieved by setting this property to zero. +This goal can be used only in 'cost' mode, where the error between the target +asymmetry and model asymmetry is squared. To make this goal suitable for +gradient-based optimization, step length values are assigned via a smoothing +function which can be controlled via 'setAsymmetrySmoothing()'. + +Users must also prescribed the stride length via 'setStrideLength()'. The goal +then calculates the minimum and maximum bounds on the distance between right +and left foot. Users must ensure that this stride length is met via problem +bounds or other goals; the value provided to MocoStepLengthAsymmetryGoal is +only used to compute the model's asymmetry in the cost function. + +Because this goal doesn't directly compute the step length asymmetry from +heel strike data, users should confirm that the step length asymmetry +from the solution matches closely to their target. To do this, we +provide the helper function computeStepAsymmetryValues() below. Users may +also want to confirm that the stride length from the optimization +matches with setStrideLength(), or set additional constraints for stride length +within the optimization. Additionally, in some cases users may want to set +target asymmetries above or below the desired value, in the event there is +some offset. +""" +def stepLengthAsymmetry(): + + + # ********************************** + # DEFINE THE OPTIMAL CONTROL PROBLEM + + # Create a MocoStudy + study = osim.MocoStudy() + study.setName("step_length_asymmetry") + + # Get the model + modelProcessor = osim.ModelProcessor("2D_gait.osim") + modelProcessor.append( + osim.ModOpTendonComplianceDynamicsModeDGF("implicit")) + + # Get the MocoProblem from the MocoStudy and set the model on it + problem = study.updProblem() + problem.setModelProcessor(modelProcessor) + + + + # ********************************** + # SET GOALS + + # Periodicity: + # All states are periodic except pelvis_tx value, lumbar actuator control + # is periodic. + periodicityGoal = osim.MocoPeriodicityGoal("periodicity") + model = modelProcessor.process() + model.initSystem() + state_names = [model.getStateVariableNames().getitem(sn) + for sn in range(model.getNumStateVariables())] + for sn in state_names: + if "pelvis_tx/value" not in sn: + periodicityGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn)) + periodicityGoal.addControlPair(osim.MocoPeriodicityGoalPair("/lumbarAct")) + problem.addGoal(periodicityGoal) + + # Average gait speed + speedGoal = osim.MocoAverageSpeedGoal("avg_speed") + speedGoal.set_desired_average_speed(1.0) + problem.addGoal(speedGoal) + + # Effort + effortGoal = osim.MocoControlGoal("effort", 10.0) + effortGoal.setExponent(3) + effortGoal.setDivideByDisplacement(True) + problem.addGoal(effortGoal) + + # Step length asymmetry: + # Create the goal, and configure it + stepLengthAsymmetryGoal = osim.MocoStepLengthAsymmetryGoal() + # Set body names for left and right foot + stepLengthAsymmetryGoal.setRightFootFrame('/bodyset/calcn_r') + stepLengthAsymmetryGoal.setLeftFootFrame('/bodyset/calcn_l') + # Provide the stride length for the simulation (m) + stepLengthAsymmetryGoal.setStrideLength(0.904) + # Value for smoothing term used to compute asymmetry (default = 5). Users + # may need to adjust this based on convergence and matching the target + # asymmetry. + stepLengthAsymmetryGoal.setAsymmetrySmoothing(5) + # Target step length asymmetry. Positive values mean greater right step + # length than left. + stepLengthAsymmetryGoal.setTargetAsymmetry(-0.10) + # Set goal weight + stepLengthAsymmetryGoal.setWeight(5) + # Add the goal to the problem + problem.addGoal(stepLengthAsymmetryGoal) + + + + # ********************************** + # SET BOUNDS + + # Set time bounds + problem.setTimeBounds(0.0, 0.94) + + # Coordinate bounds as dict + coord_bounds = {} + coord_bounds["/jointset/groundPelvis/pelvis_tilt/value"] = [-20*pi/180, + 20*pi/180] + coord_bounds["/jointset/groundPelvis/pelvis_tx/value"] = [0, 2] + coord_bounds["/jointset/groundPelvis/pelvis_ty/value"] = [0.75, 1.25] + coord_bounds["/jointset/hip_r/hip_flexion_r/value"] = [-10*pi/180, + 60*pi/180] + coord_bounds["/jointset/hip_l/hip_flexion_l/value"] = [-10*pi/180, + 60*pi/180] + coord_bounds["/jointset/knee_r/knee_angle_r/value"] = [-50*pi/180, 0] + coord_bounds["/jointset/knee_l/knee_angle_l/value"] = [-50*pi/180, 0] + coord_bounds["/jointset/ankle_r/ankle_angle_r/value"] = [-15*pi/180, + 25*pi/180] + coord_bounds["/jointset/ankle_l/ankle_angle_l/value"] = [-15*pi/180, + 25*pi/180] + coord_bounds["/jointset/lumbar/lumbar/value"] = [0, 20*pi/180] + + # Set coordinate bounds + for bnd in coord_bounds: + problem.setStateInfo(bnd, coord_bounds[bnd]) + + + + # ********************************** + # SOLVE + + # Configure the solver + solver = study.initCasADiSolver() + solver.set_num_mesh_intervals(100) + solver.set_verbosity(2) + solver.set_optim_convergence_tolerance(1e-4) + solver.set_optim_constraint_tolerance(1e-4) + solver.set_optim_max_iterations(2000) + + # Set the initial guess to be the symmetric two-steps tracking solution + # from example2DWalking.py. Run this first, or proceed without a guess. + guess_file = "walk_2D_two_steps_tracking_solution.sto" + if os.path.isfile(guess_file): + solver.setGuessFile(guess_file) + + # Print the Study to file + study.printToXML("example2DWalkingStepLengthAsymmetry.omoco") + + + # Solve + solution = study.solve() + solution.write("walk_2D_step_length_asym_solution.sto") + + # Write predicted GRF to file + contact_r = ["contactHeel_r", "contactFront_r"] + contact_l = ["contactHeel_l", "contactFront_l"] + contact_forces_table = osim.createExternalLoadsTableForGait(model, + solution, contact_r, contact_l) + osim.STOFileAdapter().write(contact_forces_table, + "walk_2D_step_length_asym_ground_forces.sto") + + # Calculate step time asymmetry + _, step_length_asym = computeStepAsymmetry(model, 25, + "walk_2D_step_length_asym_solution.sto", + "walk_2D_step_length_asym_ground_forces.sto") + print("\nStep length asymmetry: %f\n" % step_length_asym) + + + return study, solution + + + +""" +computeStepAsymmetry(): + +Calculate the values of the step length and step time asymmetry from the +results of the simulation. +""" +def computeStepAsymmetry(model_file, threshold, solution_file, grf_file): + + # Load model + model = osim.Model(model_file) + + # Load predicted GRF + grf = np.genfromtxt(grf_file, skip_header=5, delimiter="\t") + + # GRF time vector and vertical components + tvec = grf[:, 0] + + + # ********************************** + # STEP TIME ASYMMETRY + + # Find index of heel strike on each leg + hs_idxR = findHeelStrikeIndex(grf[:, 2], threshold) + hs_idxL = findHeelStrikeIndex(grf[:, 8], threshold) + + # Compute step time on each leg + hs_timeR = tvec[hs_idxR] + hs_timeL = tvec[hs_idxL] + if hs_timeR < hs_timeL: + step_timeL = hs_timeL - hs_timeR + step_timeR = tvec[-1] - step_timeL + else: + step_timeR = hs_timeR - hs_timeL + step_timeL = tvec[-1] - step_timeR + + # Calculate step time asymmetry (%) + step_time_asym = 100 * (step_timeR - step_timeL) \ + / (step_timeR + step_timeL) + + + # ********************************** + # STEP LENGTH ASYMMETRY + + # Get the states for each limb at the instant of heel strike on that limb + states_traj = osim.StatesTrajectory().createFromStatesTable(model, + osim.TimeSeriesTable(solution_file), False, True, True) + statesR = states_traj.get(hs_idxR) + statesL = states_traj.get(hs_idxL) + + # Calculate the step length + step_lengthR = calculateStepLength(model, statesR) + step_lengthL = calculateStepLength(model, statesL) + + # Calculate step length asymmetry (%) + step_length_asym = 100 * (step_lengthR - step_lengthL) \ + / (step_lengthR + step_lengthL) + + + return step_time_asym, step_length_asym + + + +""" +findHeelStrikeIndex(): + +Find heel strike index by determining foot contact on-off instances. If no +heel strike is found, then assume first index is heel strike. This +implementation differs from that of Russell T. Johnson's Matlab version, but +follows the same prinicples. +""" +def findHeelStrikeIndex(grfy, threshold): + + # Find windows representing ground contact + is_contact = (grfy > threshold).astype(int) + + # Calculate transition points, i.e. heel strike (1) and foot off (-1) + contact_diff = np.diff(np.insert(is_contact, 0, 1)) + + # Extract heel strike and foot off indices. If no heel strike found, + # assume first index is heel strike. + idxs = np.where(contact_diff == 1)[0] + if idxs.size == 0: + idx = 0 + else: + idx = idxs[0] + + return int(idx) + + + +""" +calculateStepLength(): + +Find step length by configuring the model at heel strike, then compute distance +between contact spheres along the fore-aft coordinate. +""" +def calculateStepLength(model, state): + + # Configure the model at heel strike + model.initSystem() + model.realizePosition(state) + + # Get the heel contact spheres + contact_r = model.getContactGeometrySet().get("heel_r") + contact_l = model.getContactGeometrySet().get("heel_l") + + # Find the positions of the contact spheres in the global frame + pos_r = contact_r.getFrame().getPositionInGround(state) + pos_l = contact_l.getFrame().getPositionInGround(state) + + # Step length is the difference between global position of the left and + # right along the fore-aft coordinate (x) + step_length = abs(pos_r.get(0) - pos_l.get(0)) + + return step_length + + + + +# %% STEP TIME ASYMMETRY + +# Solve and visualise +step_time_study, step_time_solution = stepTimeAsymmetry() +step_time_study.visualize(step_time_solution) + + + +# %% STEP LENGTH ASYMMETRY + +# Solve and visualise +step_length_study, step_length_solution = stepLengthAsymmetry() +step_length_study.visualize(step_length_solution) + From c50b1bd2f974114fe007a1f34d8634cdd7e20bd8 Mon Sep 17 00:00:00 2001 From: psbiomech Date: Mon, 27 Jun 2022 15:52:47 +1000 Subject: [PATCH 5/8] Delete example2DWalking.py --- Bindings/Python/examples/example2DWalking.py | 416 ------------------- 1 file changed, 416 deletions(-) delete mode 100644 Bindings/Python/examples/example2DWalking.py diff --git a/Bindings/Python/examples/example2DWalking.py b/Bindings/Python/examples/example2DWalking.py deleted file mode 100644 index 4a788cd379..0000000000 --- a/Bindings/Python/examples/example2DWalking.py +++ /dev/null @@ -1,416 +0,0 @@ -# -*- coding: utf-8 -*- -""" -MOCO: WALKING 2D EXAMPLE - TRACKING & PREDICTION - -@author: Prasanna Sritharan, June 2022 - -This is a Python implementation of an example optimal control -problem (2D walking) orginally created in C++ by Antoine Falisse -(see: example2DWalking.cpp) and adapted for Matlab by Brian Umberger -(see: example2DWalking.m). -""" - -from math import pi -import opensim as osim - - - - -""" -gait_tracking(): -Set up a coordinate tracking problem where the goal is to minimize the -difference between provided and simulated coordinate values and speeds (and -ground reaction forces), as well as to minimize an effort cost (squared -controls). The provided data represents one step. Endpoint constraints -enforce periodicity of the coordinate values (except for pelvis tx) and speeds, -coordinate actuator controls, and muscle activations. -""" -def gait_tracking(): - - - - # ********************************** - # DEFINE THE OPTIMAL CONTROL PROBLEM - - # Create tracking problem - track = osim.MocoTrack() - track.setName("gait_tracking") - - # Construct a ModelProcessor and set it on the tool - modelProcessor = osim.ModelProcessor("2D_gait.osim") - track.setModel(modelProcessor) - - - # Get the coordinates into a TableProcessor - tableProcessor = osim.TableProcessor("referenceCoordinates.sto") - tableProcessor.append(osim.TabOpLowPassFilter(6.0)) - - # Set the coordinates as the reference states - track.setStatesReference(tableProcessor) - track.set_allow_unused_references(True) - - # Only coordinates are provided so derive to get speeds and track these too - track.set_track_reference_position_derivatives(True) - - # use coordinates and speeds for initial guess - track.set_apply_tracked_states_to_guess(True) - - # Define the global state tracking weight - track.set_states_global_tracking_weight(1.0) - - - # Set initial time, final time and mesh interval - track.set_initial_time(0.0) - track.set_final_time(0.47008941) - - # Call initialize() to receive a pre-configured MocoStudy object based on - # the settings above. Use this to customize the problem beyond the - # Mocotrack interface. - study = track.initialize() - - # Get a writable reference to the MocoProblem from the MocoStudy to perform - # more customisation - problem = study.updProblem() - - - - # ********************************** - # SET GOALS - - # Symmetry: - # This goal allows us to simulate only one step with left-right symmetry - # that we can then double to create two steps, one on each leg - # (IFO>IFS>CFO>CFS). Note that this is not actually a full gait cycle - # (IFO>IFS>CFO>CFS>IFO). - symmetryGoal = osim.MocoPeriodicityGoal("symmetry_goal") - problem.addGoal(symmetryGoal) - - # Enforce periodic symmetry - model = modelProcessor.process() - model.initSystem() - state_names = [model.getStateVariableNames().getitem(sn) - for sn in range(model.getNumStateVariables())] - for sn in state_names: - - # Symmetric coordinate values and speeds (except for pelvis_tx value): - # Here we constrain final coordinate values of one leg to match the - # initial value of the other leg. Manually add pelvis_tx speed later. - if "jointset" in sn: - if "_r" in sn: - symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, - sn.replace("_r", "_l"))) - elif "_l" in sn: - symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, - sn.replace("_l", "_r"))) - elif "pelvis_tx" not in sn: - symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn)) - - # Symmetric muscle activations: - # Here, we constrain final muscle activation values of one leg to match - # the initial activation values of the other leg. - elif "activation" in sn: - if "_r" in sn: - symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, - sn.replace("_r", "_l"))) - elif "_l" in sn: - symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, - sn.replace("_l", "_r"))) - - - # The pelvis_tx speed has periodic symmetry - symmetryGoal.addStatePair( - osim.MocoPeriodicityGoalPair("/jointset/groundPelvis/pelvis_tx/speed")) - - # Lumbar control has periodic symmetry. The other controls don't need - # symmetry enforced as they are all muscle excitations. Their behaviour - # will be contstrained by the periodicity imposed on their respective - # activations. - symmetryGoal.addControlPair(osim.MocoPeriodicityGoalPair('/lumbarAct')) - - - # Effort: - # Get a reference to the MocoControlGoal that is added to a MocoTrack - # problem by default and adjust the weight - effort = osim.MocoControlGoal.safeDownCast( - problem.updGoal("control_effort")) - effort.setWeight(10.0) - - - # Ground contact: - # Track the left and right vertical and fore-aft GRFs - contact_r = ["contactHeel_r", "contactFront_r"] - contact_l = ["contactHeel_l", "contactFront_l"] - grf_tracking_weight = 1 - if grf_tracking_weight != 0: - - # Create a contact tracking goal - contactTracking = osim.MocoContactTrackingGoal("contact", - grf_tracking_weight) - contactTracking.setExternalLoadsFile("referenceGRF.xml") - - # Add contact groups. The sum of all the contact forces in each group - # should track the force data from a single ExternalForce - contactTracking.addContactGroup(contact_r, "Right_GRF") - contactTracking.addContactGroup(contact_l, "Left_GRF") - - # 2D walking problem so consider force errors in the sagittal plane - contactTracking.setProjection("plane") - contactTracking.setProjectionVector(osim.Vec3(0, 0, 1)) - - # add the goal to the MocoProblem - problem.addGoal(contactTracking) - - - - # ********************************** - # SET BOUNDS - - # Coordinate bounds as dict - coord_bounds = {} - coord_bounds["/jointset/groundPelvis/pelvis_tilt/value"] = [-20*pi/180, - -10*pi/180] - coord_bounds["/jointset/groundPelvis/pelvis_tx/value"] = [0.0, 1.0] - coord_bounds["/jointset/groundPelvis/pelvis_ty/value"] = [0.75, 1.25] - coord_bounds["/jointset/hip_r/hip_flexion_r/value"] = [-10*pi/180, - 60*pi/180] - coord_bounds["/jointset/hip_l/hip_flexion_l/value"] = [-10*pi/180, - 60*pi/180] - coord_bounds["/jointset/knee_r/knee_angle_r/value"] = [-50*pi/180, 0] - coord_bounds["/jointset/knee_l/knee_angle_l/value"] = [-50*pi/180, 0] - coord_bounds["/jointset/ankle_r/ankle_angle_r/value"] = [-15*pi/180, - 25*pi/180] - coord_bounds["/jointset/ankle_l/ankle_angle_l/value"] = [-15*pi/180, - 25*pi/180] - coord_bounds["/jointset/lumbar/lumbar/value"] = [0, 20*pi/180] - - # Set coordinate bounds - for bnd in coord_bounds: - problem.setStateInfo(bnd, coord_bounds[bnd]); - - - - # ********************************** - # SOLVE - - # The Solver is pre-configured, however, uncomment below to configure - # further if desired. This gets a writable reference to the Solver, for - # custom configuration. - # solver = study.updSolver() - # solver.set_num_mesh_intervals(50); - # solver.set_verbosity(2); - # solver.set_optim_solver("ipopt"); - # solver.set_optim_convergence_tolerance(1e-4); - # solver.set_optim_constraint_tolerance(1e-4); - # solver.set_optim_max_iterations(1000); - - - # Solve the problem for a single step - solution = study.solve() - - - # Create two mirrored steps from the single step solution, see API - # documentation for use of this utility function - two_steps_solution = osim.createPeriodicTrajectory(solution) - two_steps_solution.write("walk_2D_two_steps_tracking_solution.sto") - - # Also extract the predicted ground forces, see API documentation for use - # of this utility function - contact_forces_table = osim.createExternalLoadsTableForGait(model, - two_steps_solution, contact_r, contact_l) - osim.STOFileAdapter().write(contact_forces_table, - "walk_2D_two_steps_tracking_ground_forces.sto") - - - return study, solution, two_steps_solution - - - - -""" -gait_prediction(tracking_solution): -Set up a gait prediction problem where the goal is to minimize effort -(squared controls) divided by distance traveled while enforcing symmetry of -the walking cycle and a prescribed average gait speed through endpoint -constraints. The solution of the coordinate tracking problem is used as an -initial guess for the prediction. -""" -def gait_prediction(tracking_solution): - - - - # ********************************** - # DEFINE THE OPTIMAL CONTROL PROBLEM - - # Create predcition problem - study = osim.MocoStudy() - study.setName("gait_prediciton") - - # Get a writable reference to the MocoProblem from the MocoStudy to - # customise the problem settings - problem = study.updProblem() - - # Construct a ModelProcessor and set it on the Problem - modelProcessor = osim.ModelProcessor("2D_gait.osim") - problem.setModelProcessor(modelProcessor) - - - - # ********************************** - # SET GOALS - - # Symmetry: This goal allows us to simulate only one step with left-right - # symmetry that we can then double to create two steps, one on each leg - # (IFO>IFS>CFO>CFS). Note that this is not actually a full gait cycle - # (IFO>IFS>CFO>CFS>IFO) - symmetryGoal = osim.MocoPeriodicityGoal("symmetry_goal") - problem.addGoal(symmetryGoal) - - - # Enforce periodic symmetry - model = modelProcessor.process() - model.initSystem() - state_names = [model.getStateVariableNames().getitem(sn) - for sn in range(model.getNumStateVariables())] - for sn in state_names: - - # Symmetric coordinate values and speeds (except for pelvis_tx value): - # Here we constrain final coordinate values of one leg to match the - # initial value of the other leg. Manually add pelvis_tx speed later. - if "jointset" in sn: - if "_r" in sn: - symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, - sn.replace("_r", "_l"))) - elif "_l" in sn: - symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, - sn.replace("_l", "_r"))) - elif "pelvis_tx" not in sn: - symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn)) - - # Symmetric muscle activations: - # Here, we constrain final muscle activation values of one leg to match - # the initial activation values of the other leg. - elif "activation" in sn: - if "_r" in sn: - symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, - sn.replace("_r", "_l"))) - elif "_l" in sn: - symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, - sn.replace("_l", "_r"))) - - # The pelvis_tx speed has periodic symmetry - symmetryGoal.addStatePair( - osim.MocoPeriodicityGoalPair("/jointset/groundPelvis/pelvis_tx/speed")) - - # Lumbar control has periodic symmetry. The other controls don't need - # symmetry enforces as they are all muscle excitations. Their behaviour - # will be modulated by the periodicity imposed on their activations. - symmetryGoal.addControlPair(osim.MocoPeriodicityGoalPair('/lumbarAct')) - - # Specify the desired average walk speed, add as a goal - speedGoal = osim.MocoAverageSpeedGoal('avg_speed') - speedGoal.set_desired_average_speed(1.2) - problem.addGoal(speedGoal) - - # Minimise total "effort" over the distance, i.e. minimise sum of the - # absolute values of all controls raised to a given exponent, integrated - # over the phase. In a MocoTrack problem, this is automatically added, - # however, in this predictive problem, we have created a MocoStudy from - # scratch, so we need to add this goal manually to the Problem. The second - # input argument to the constructor is the weight applied to this goal. - # We also normalise the effort by the distance travelled. - effortGoal = osim.MocoControlGoal('effort', 10) - effortGoal.setExponent(3) - effortGoal.setDivideByDisplacement(True) - problem.addGoal(effortGoal) - - - - # ********************************** - # SET BOUNDS - - # set time bounds - problem.setTimeBounds(0, [0.4, 0.6]) - - - # Coordinate bounds as dict - coord_bounds = {} - coord_bounds["/jointset/groundPelvis/pelvis_tilt/value"] = [-20*pi/180, - -10*pi/180] - coord_bounds["/jointset/groundPelvis/pelvis_tx/value"] = [0.0, 1.0] - coord_bounds["/jointset/groundPelvis/pelvis_ty/value"] = [0.75, 1.25] - coord_bounds["/jointset/hip_r/hip_flexion_r/value"] = [-10*pi/180, - 60*pi/180] - coord_bounds["/jointset/hip_l/hip_flexion_l/value"] = [-10*pi/180, - 60*pi/180] - coord_bounds["/jointset/knee_r/knee_angle_r/value"] = [-50*pi/180, 0] - coord_bounds["/jointset/knee_l/knee_angle_l/value"] = [-50*pi/180, 0] - coord_bounds["/jointset/ankle_r/ankle_angle_r/value"] = [-15*pi/180, - 25*pi/180] - coord_bounds["/jointset/ankle_l/ankle_angle_l/value"] = [-15*pi/180, - 25*pi/180] - coord_bounds["/jointset/lumbar/lumbar/value"] = [0, 20*pi/180] - - # Set coordinate bounds - for bnd in coord_bounds: - problem.setStateInfo(bnd, coord_bounds[bnd]) - - - - # ********************************** - # SOLVE - - # Configure the solver. In the tracking problem, the solver was - # pre-configured, using MocoTrack, however, as we have created a MocoStudy - # from scratch, we need to initialise the MocoSolver and configure it. - solver = study.initCasADiSolver() - solver.set_num_mesh_intervals(50) - solver.set_verbosity(2) - solver.set_optim_solver("ipopt") - solver.set_optim_convergence_tolerance(1e-4) - solver.set_optim_constraint_tolerance(1e-4) - solver.set_optim_max_iterations(1000) - - # Set the tracking solution for one step as the initial guess - solver.setGuess(tracking_solution) - - - # Solve the problem for a single step - solution = study.solve() - - - # Create two mirrored steps from the single step solution, see API - # documentation for use of this utility function - two_steps_solution = osim.createPeriodicTrajectory(solution) - two_steps_solution.write("walk_2D_two_steps_prediction_solution.sto") - - # Also extract the predicted ground forces, see API documentation for use - # of this utility function - contact_r = ["contactHeel_r", "contactFront_r"] - contact_l = ["contactHeel_l", "contactFront_l"] - contact_forces_table = osim.createExternalLoadsTableForGait(model, - two_steps_solution, contact_r, contact_l) - osim.STOFileAdapter().write(contact_forces_table, - "walk_2D_two_steps_prediction_ground_forces.sto") - - - return study, solution, two_steps_solution - - - -# %% TRACKING PROBLEM - -# Solve the problem and visualise -tracking_study, tracking_solution, tracking_two_steps_solution = \ - gait_tracking() -tracking_study.visualize(tracking_two_steps_solution) - - - -# %% PREDICTION PROBLEM - -# Solve the problem and visualise (uses tracking_solution as initial guess) -prediction_study, prediction_solution, prediction_two_steps_solution = \ - gait_prediction(tracking_solution) -prediction_study.visualize(prediction_two_steps_solution) - - From 1c8353b10c8333eac30ed790ae750ff9e66aabdf Mon Sep 17 00:00:00 2001 From: psbiomech Date: Mon, 27 Jun 2022 15:52:59 +1000 Subject: [PATCH 6/8] Delete example2DWalkingMetabolics.py --- .../examples/example2DWalkingMetabolics.py | 219 ------------------ 1 file changed, 219 deletions(-) delete mode 100644 Bindings/Python/examples/example2DWalkingMetabolics.py diff --git a/Bindings/Python/examples/example2DWalkingMetabolics.py b/Bindings/Python/examples/example2DWalkingMetabolics.py deleted file mode 100644 index 88b36600cf..0000000000 --- a/Bindings/Python/examples/example2DWalkingMetabolics.py +++ /dev/null @@ -1,219 +0,0 @@ -# -*- coding: utf-8 -*- -""" -MOCO: WALKING 2D EXAMPLE - COORDINATE TRACKING, MINIMISE METABOLIC COST - -@author: Prasanna Sritharan, June 2022 - -This is a Python implementation of an example optimal control -problem (2D walking) orginally created in C++ by Antoine Falisse -(see: example2DWalkingMetabolics.cpp) and adapted for Matlab by Brian Umberger -(see: example2DWalkingMetabolics.m). - -This example features a tracking simulation of walking that includes -minimisation of the metabolic cost of transport computed using a smooth -approximation of the metabolic energy model of Bhargava et al (2004). - -Set a coordinate tracking problem where the goal is to minimize the -difference between provided and simulated coordinate values and speeds -as well as to minimize an effort cost (squared controls) and a metabolic -cost (metabolic energy normalized by distance traveled and body mass; -the metabolics model is based on a smooth approximation of the -phenomenological model described by Bhargava et al. (2004)). The provided -data represents half a gait cycle. Endpoint constraints enforce periodicity -of the coordinate values (except for pelvis tx) and speeds, coordinate -actuator controls, and muscle activations. -""" - - -from math import pi -import opensim as osim - - - -# %% SET THE MODEL AND METABOLICS ANALYSIS - -# Get the OpenSim model -model = osim.Model("2D_gait.osim") - -# Add a component to calculate metabolics -metabolics = osim.Bhargava2004SmoothedMuscleMetabolics() -metabolics.setName("metabolics") -metabolics.set_use_smoothing(True) - -# Add muscles to metabolics component -leg = ["r", "l"] -musclist = ["hamstrings", "bifemsh", "glut_max", "iliopsoas", "rect_fem", - "vasti", "gastroc", "soleus", "tib_ant"] -for mc in [m + "_" + l for m in musclist for l in leg]: - metabolics.addMuscle(mc, osim.Muscle.safeDownCast(model.getComponent(mc))) - -# Add metabolics component to model -model.addComponent(metabolics) -model.finalizeConnections() - - - -# %% DEFINE THE OPTIMAL CONTROL PROBLEM - -# Create the tracking problem -track = osim.MocoTrack() -track.setName("gait_tracking_met_cost") - -# Pass the model to MocoTrack -modelProcessor = osim.ModelProcessor(model) -track.setModel(modelProcessor) - -# Reference data for coordinate tracking -tableProcessor = osim.TableProcessor("referenceCoordinates.sto") -tableProcessor.append(osim.TabOpLowPassFilter(6.0)) -track.setStatesReference(tableProcessor) - -# Remaining MocoTrack settings -track.set_states_global_tracking_weight(30) -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(0.0) -track.set_final_time(0.47008941) - -# Call initialize() to get the internal MocoStudy. The MocoStudy is comprised -# of a MocoProblem and a MocoSolver. Get the MocoProblem to configure further. -study = track.initialize() -problem = study.updProblem() - - - -# %% SET GOALS - -# Symmetry: -# This goal allows us to simulate only one step with left-right symmetry that -# we can then double to create two steps, one on each leg (IFO>IFS>CFO>CFS). -# Note that this is not actually a full gait cycle (IFO>IFS>CFO>CFS>IFO). -symmetryGoal = osim.MocoPeriodicityGoal("symmetry_goal") -problem.addGoal(symmetryGoal) - -# Enforce periodic symmetry -model = modelProcessor.process() -model.initSystem() -state_names = [model.getStateVariableNames().getitem(sn) - for sn in range(model.getNumStateVariables())] -for sn in state_names: - - # Symmetric coordinate values and speeds (except for pelvis_tx value): - # Here we constrain final coordinate values of one leg to match the initial - # value of the other leg. Manually add pelvis_tx speed later. - if "jointset" in sn: - if "_r" in sn: - symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, - sn.replace("_r", "_l"))) - elif "_l" in sn: - symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, - sn.replace("_l", "_r"))) - elif "pelvis_tx" not in sn: - symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn)) - - # Symmetric muscle activations: - # Here, we constrain final muscle activation values of one leg to match the - # initial activation values of the other leg. - elif "activation" in sn: - if "_r" in sn: - symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, - sn.replace("_r", "_l"))) - elif "_l" in sn: - symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, - sn.replace("_l", "_r"))) - - -# The pelvis_tx speed has periodic symmetry -symmetryGoal.addStatePair( - osim.MocoPeriodicityGoalPair("/jointset/groundPelvis/pelvis_tx/speed")) - -# Lumbar control has periodic symmetry. The other controls don't need symmetry -# enforced as they are all muscle excitations. Their behaviour will be -# modulated by the periodicity imposed on their respective activations. -symmetryGoal.addControlPair(osim.MocoPeriodicityGoalPair('/lumbarAct')) - - -# Effort: -# Get a reference to the MocoControlGoal that is added to a MocoTrack -# problem by default and adjust the weight -effort = osim.MocoControlGoal().safeDownCast(problem.updGoal("control_effort")) -effort.setWeight(0.1) - - -# Metabolic cost: -# Total metabolic rate includes activation heat rate, maintenance heat rate, -# shortening heat rate, mechanical work rate, and basal metabolic rate. -met_weight = 0.1 -metabolicsGoal = osim.MocoOutputGoal("metabolics", met_weight) -metabolicsGoal.setOutputPath("/metabolics|total_metabolic_rate") -metabolicsGoal.setDivideByDisplacement(True) -metabolicsGoal.setDivideByMass(True) -problem.addGoal(metabolicsGoal) - - - -# %% SET BOUNDS - -# Coordinate bounds as dict -coord_bounds = {} -coord_bounds["/jointset/groundPelvis/pelvis_tilt/value"] = [-20*pi/180, - -10*pi/180] -coord_bounds["/jointset/groundPelvis/pelvis_tx/value"] = [0.0, 1.0] -coord_bounds["/jointset/groundPelvis/pelvis_ty/value"] = [0.75, 1.25] -coord_bounds["/jointset/hip_r/hip_flexion_r/value"] = [-10*pi/180, - 60*pi/180] -coord_bounds["/jointset/hip_l/hip_flexion_l/value"] = [-10*pi/180, - 60*pi/180] -coord_bounds["/jointset/knee_r/knee_angle_r/value"] = [-50*pi/180, 0] -coord_bounds["/jointset/knee_l/knee_angle_l/value"] = [-50*pi/180, 0] -coord_bounds["/jointset/ankle_r/ankle_angle_r/value"] = [-15*pi/180, - 25*pi/180] -coord_bounds["/jointset/ankle_l/ankle_angle_l/value"] = [-15*pi/180, - 25*pi/180] -coord_bounds["/jointset/lumbar/lumbar/value"] = [0, 20*pi/180] - -# Set coordinate bounds -for bnd in coord_bounds: - problem.setStateInfo(bnd, coord_bounds[bnd]); - - - -# %% SOLVE - -# Configure the solver -solver = study.initCasADiSolver() -solver.set_num_mesh_intervals(50) -solver.set_verbosity(2) -solver.set_optim_solver("ipopt") -solver.set_optim_convergence_tolerance(1e-4) -solver.set_optim_constraint_tolerance(1e-4) -solver.set_optim_max_iterations(10000) - -# Solve the problem for a single step -solution = study.solve() - - -# Create two mirrored steps from the single step solution, see API -# documentation for use of this utility function -two_steps_solution = osim.createPeriodicTrajectory(solution) -two_steps_solution.write("walk_2D_metabolics_two_steps_tracking_solution.sto") - -# Also extract the predicted ground forces, see API documentation for use of -# this utility function -contact_r = ["contactHeel_r", "contactFront_r"] -contact_l = ["contactHeel_l", "contactFront_l"] -contact_forces_table = osim.createExternalLoadsTableForGait(model, - two_steps_solution, contact_r, contact_l) -osim.STOFileAdapter().write(contact_forces_table, - "walk_2D_metabolics_two_steps_tracking_ground_forces.sto") - - -# Determine the cost of transport from the metabolics cost term. Need to divide -# by the weight applied earlier. -COT = solution.getObjectiveTerm("metabolics") / met_weight -print("\nThe metabolic cost of transport is: %f" % COT) - - -# visualise -study.visualize(two_steps_solution) \ No newline at end of file From 574bbfa5553f6bf79c788e9604e2a6bf81aa16b1 Mon Sep 17 00:00:00 2001 From: psbiomech Date: Mon, 27 Jun 2022 15:53:10 +1000 Subject: [PATCH 7/8] Delete example2DWalkingStepAsymmetry.py --- .../examples/example2DWalkingStepAsymmetry.py | 537 ------------------ 1 file changed, 537 deletions(-) delete mode 100644 Bindings/Python/examples/example2DWalkingStepAsymmetry.py diff --git a/Bindings/Python/examples/example2DWalkingStepAsymmetry.py b/Bindings/Python/examples/example2DWalkingStepAsymmetry.py deleted file mode 100644 index fd8fdfdefe..0000000000 --- a/Bindings/Python/examples/example2DWalkingStepAsymmetry.py +++ /dev/null @@ -1,537 +0,0 @@ -# -*- coding: utf-8 -*- -""" -MOCO: WALKING 2D EXAMPLE - STEP ASYMMETRY - -@author: Prasanna Sritharan, June 2022 - - -This is a Python implementation of an example optimal control problem -originally written by Russell T. Johnson for Matlab -(see example2DWalkingStepAsymmetry.m). - -Simulate asymmetric gait using MocoStepTimeAsymmetryGoal and -MocoStepLengthAsymmetryGoal. This is an extension of the example2DWalking -MATLAB example (see example2DWalking.m for details about model and data used). -""" - - -from math import pi -import opensim as osim -import os -import numpy as np - - - -""" -stepTimeAsymmetry(): - -Set up a predictive optimization problem where the goal is to minimize an -effort cost (cubed controls) and hit a target step time asymmetry. Unlike -example2DWalking, this problem requires simulating a full gait cycle. -Additionally, endpoint constraints enforce periodicity of the coordinate values -(except for pelvis tx) and speeds, coordinate actuator controls, and muscle -activations. - -Step time is defined as the time between consecutive foot strikes. Step Time -Asymmetry (STA) is a ratio and is calculated as follows: - - Right Step Time (RST) = Time from left foot-strike to right foot-strike - - Left Step Time (LST) = Time from right foot-strike to left foot-strike - - STA = (RST - LST) / (RST + LST) - -The step time goal works by "counting" the number of nodes that each foot is in -contact with the ground (with respect to a specified contact force threshold). -Since, in walking, there are double support phases where both feet are on the -ground, the goal also detects which foot is in front and assigns the step time -to the leading foot. Altogether, it estimates the time between consecutive -heel strikes in order to infer the left and right step times. - -The contact elements for each foot must specified via 'setLeftContactGroup()' -and 'setRightContactGroup()'. The force element and force threshold used to -determine when a foot is in contact is set via 'setContactForceDirection()' and -'setContactForceThreshold()'. - -Users must provide the target asymmetry value via 'setTargetAsymmetry()'. -Asymmetry values ranges from -1.0 to 1.0. For example, 0.20 is 20positive -step time asymmetry with greater right step times than left step times. A -symmetric step times solution can be achieved by setting this property to zero. -This goal can be used only in 'cost' mode, where the error between the target -asymmetry and model asymmetry is squared. To make this goal suitable for -gradient-based optimization, step time values are assigned via smoothing -functions which can be controlled via 'setAsymmetrySmoothing()' and -'setContactDetectionSmoothing()'. - -Since this goal doesn't directly compute the step time asymmetry from heel -strikes, users should confirm that the step time asymmetry from the solution -matches closely to the target. To do this, we provide the helper function -computeStepAsymmetryValues() below. -""" -def stepTimeAsymmetry(): - - - # ********************************** - # DEFINE THE OPTIMAL CONTROL PROBLEM - - # Create a MocoStudy - study = osim.MocoStudy() - study.setName("step_time_asymmetry") - - # Get the model - modelProcessor = osim.ModelProcessor("2D_gait.osim") - modelProcessor.append( - osim.ModOpTendonComplianceDynamicsModeDGF("implicit")) - - # Get the MocoProblem from the MocoStudy and set the model on it - problem = study.updProblem() - problem.setModelProcessor(modelProcessor) - - - - # ********************************** - # SET GOALS - - # Periodicity: - # All states are periodic except pelvis_tx value, lumbar actuator control - # is periodic. - periodicityGoal = osim.MocoPeriodicityGoal("periodicity") - model = modelProcessor.process() - model.initSystem() - state_names = [model.getStateVariableNames().getitem(sn) - for sn in range(model.getNumStateVariables())] - for sn in state_names: - if "pelvis_tx/value" not in sn: - periodicityGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn)) - periodicityGoal.addControlPair(osim.MocoPeriodicityGoalPair("/lumbarAct")) - problem.addGoal(periodicityGoal) - - # Average gait speed - speedGoal = osim.MocoAverageSpeedGoal("avg_speed") - speedGoal.set_desired_average_speed(1.0) - problem.addGoal(speedGoal) - - # Effort - effortGoal = osim.MocoControlGoal("effort", 10.0) - effortGoal.setExponent(3) - effortGoal.setDivideByDisplacement(True) - problem.addGoal(effortGoal) - - # Step time asymmetry: - # Create the goal, and configure it - stepTimeAsymmetryGoal = \ - osim.MocoStepTimeAsymmetryGoal("step_time_asymmetry") - # Value for smoothing term used to compute when foot contact is made - # (default = 0.25). Users may need to adjust this based on convergence and - # matching the target asymmetry. - stepTimeAsymmetryGoal.setContactDetectionSmoothing(0.4) - # Contact threshold based on vertical GRF (default = 25 N) - stepTimeAsymmetryGoal.setContactForceThreshold(25.0) - # Value for smoothing term used to compute asymmetry (default = 10). Users - # may need to adjust this based on convergence and matching the target - # asymmetry. - stepTimeAsymmetryGoal.setAsymmetrySmoothing(3.0) - # Target step length asymmetry. Positive values mean greater right step - # length than left. - stepTimeAsymmetryGoal.setTargetAsymmetry(0.10) - # Set goal weight - stepTimeAsymmetryGoal.setWeight(5.0) - # Need to define the names of the left and right heel spheres: this is - # used to detect which foot is in front during double support phase. - contact_r = ["contactHeel_r", "contactFront_r"] - contact_l = ["contactHeel_l", "contactFront_l"] - stepTimeAsymmetryGoal.setRightContactGroup(contact_r, "contactHeel_r") - stepTimeAsymmetryGoal.setLeftContactGroup(contact_l, "contactHeel_l") - # Add the goal to the problem - problem.addGoal(stepTimeAsymmetryGoal) - - - - # ********************************** - # SET BOUNDS - - # Set time bounds - problem.setTimeBounds(0.0, 0.94) - - # Coordinate bounds as dict - coord_bounds = {} - coord_bounds["/jointset/groundPelvis/pelvis_tilt/value"] = [-20*pi/180, - 20*pi/180] - coord_bounds["/jointset/groundPelvis/pelvis_tx/value"] = [0, 2] - coord_bounds["/jointset/groundPelvis/pelvis_ty/value"] = [0.75, 1.25] - coord_bounds["/jointset/hip_r/hip_flexion_r/value"] = [-10*pi/180, - 60*pi/180] - coord_bounds["/jointset/hip_l/hip_flexion_l/value"] = [-10*pi/180, - 60*pi/180] - coord_bounds["/jointset/knee_r/knee_angle_r/value"] = [-50*pi/180, 0] - coord_bounds["/jointset/knee_l/knee_angle_l/value"] = [-50*pi/180, 0] - coord_bounds["/jointset/ankle_r/ankle_angle_r/value"] = [-15*pi/180, - 25*pi/180] - coord_bounds["/jointset/ankle_l/ankle_angle_l/value"] = [-15*pi/180, - 25*pi/180] - coord_bounds["/jointset/lumbar/lumbar/value"] = [0, 20*pi/180] - - # Set coordinate bounds - for bnd in coord_bounds: - problem.setStateInfo(bnd, coord_bounds[bnd]) - - - - # ********************************** - # SOLVE - - # Configure the solver - solver = study.initCasADiSolver() - solver.set_num_mesh_intervals(100) - solver.set_verbosity(2) - solver.set_optim_convergence_tolerance(1e-4) - solver.set_optim_constraint_tolerance(1e-4) - solver.set_optim_max_iterations(2000) - - # Set the initial guess to be the symmetric two-steps tracking solution - # from example2DWalking.py. Run this first, or proceed without a guess. - guess_file = "walk_2D_two_steps_tracking_solution.sto" - if os.path.isfile(guess_file): - solver.setGuessFile(guess_file) - - # Print the Study to file - study.printToXML("example2DWalkingStepTimeAsymmetry.omoco") - - - # Solve - solution = study.solve() - solution.write("walk_2D_step_time_asym_solution.sto") - - - # Write predicted GRF to file - contact_forces_table = osim.createExternalLoadsTableForGait(model, - solution, contact_r, contact_l) - osim.STOFileAdapter().write(contact_forces_table, - "walk_2D_step_time_asym_ground_forces.sto") - - # Calculate step time asymmetry - step_time_asym, _ = computeStepAsymmetry(model, 25, - "walk_2D_step_time_asym_solution.sto", - "walk_2D_step_time_asym_ground_forces.sto") - print("\nStep time asymmetry: %f\n" % step_time_asym) - - - return study, solution - - - -""" -stepLengthAsymmetry(): - -This goal works by limiting the distance between feet, or "foot frames", -throughout the gait cycle. The goal calculates the distance between the left -foot and right foot, then limits the distance between feet to not pass beyond -minimum (negative) or maximum (positive) bounds. There are two limits used: -one that limits the distance between feet when the right foot is in front, and -one that limits the distance between feet when the left foot is in front. - -Step Length Asymmetry (SLA) is a ratio and is calculated as follows: -The Right Step Length (RSL) is the distance between feet at right foot strike -The Left Step Length (LSL) is the distance between feet at left foot strike -Step Length Asymmetry = (RSL - LSL)/ (RSL + LSL) - -Users must provide the target asymmetry value via 'setTargetAsymmetry()'. -Asymmetry values ranges from -1.0 to 1.0. For example, 0.20 is 20positive -step length asymmetry with greater right step length than left step length. A -symmetric step length solution can be achieved by setting this property to zero. -This goal can be used only in 'cost' mode, where the error between the target -asymmetry and model asymmetry is squared. To make this goal suitable for -gradient-based optimization, step length values are assigned via a smoothing -function which can be controlled via 'setAsymmetrySmoothing()'. - -Users must also prescribed the stride length via 'setStrideLength()'. The goal -then calculates the minimum and maximum bounds on the distance between right -and left foot. Users must ensure that this stride length is met via problem -bounds or other goals; the value provided to MocoStepLengthAsymmetryGoal is -only used to compute the model's asymmetry in the cost function. - -Because this goal doesn't directly compute the step length asymmetry from -heel strike data, users should confirm that the step length asymmetry -from the solution matches closely to their target. To do this, we -provide the helper function computeStepAsymmetryValues() below. Users may -also want to confirm that the stride length from the optimization -matches with setStrideLength(), or set additional constraints for stride length -within the optimization. Additionally, in some cases users may want to set -target asymmetries above or below the desired value, in the event there is -some offset. -""" -def stepLengthAsymmetry(): - - - # ********************************** - # DEFINE THE OPTIMAL CONTROL PROBLEM - - # Create a MocoStudy - study = osim.MocoStudy() - study.setName("step_length_asymmetry") - - # Get the model - modelProcessor = osim.ModelProcessor("2D_gait.osim") - modelProcessor.append( - osim.ModOpTendonComplianceDynamicsModeDGF("implicit")) - - # Get the MocoProblem from the MocoStudy and set the model on it - problem = study.updProblem() - problem.setModelProcessor(modelProcessor) - - - - # ********************************** - # SET GOALS - - # Periodicity: - # All states are periodic except pelvis_tx value, lumbar actuator control - # is periodic. - periodicityGoal = osim.MocoPeriodicityGoal("periodicity") - model = modelProcessor.process() - model.initSystem() - state_names = [model.getStateVariableNames().getitem(sn) - for sn in range(model.getNumStateVariables())] - for sn in state_names: - if "pelvis_tx/value" not in sn: - periodicityGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn)) - periodicityGoal.addControlPair(osim.MocoPeriodicityGoalPair("/lumbarAct")) - problem.addGoal(periodicityGoal) - - # Average gait speed - speedGoal = osim.MocoAverageSpeedGoal("avg_speed") - speedGoal.set_desired_average_speed(1.0) - problem.addGoal(speedGoal) - - # Effort - effortGoal = osim.MocoControlGoal("effort", 10.0) - effortGoal.setExponent(3) - effortGoal.setDivideByDisplacement(True) - problem.addGoal(effortGoal) - - # Step length asymmetry: - # Create the goal, and configure it - stepLengthAsymmetryGoal = osim.MocoStepLengthAsymmetryGoal() - # Set body names for left and right foot - stepLengthAsymmetryGoal.setRightFootFrame('/bodyset/calcn_r') - stepLengthAsymmetryGoal.setLeftFootFrame('/bodyset/calcn_l') - # Provide the stride length for the simulation (m) - stepLengthAsymmetryGoal.setStrideLength(0.904) - # Value for smoothing term used to compute asymmetry (default = 5). Users - # may need to adjust this based on convergence and matching the target - # asymmetry. - stepLengthAsymmetryGoal.setAsymmetrySmoothing(5) - # Target step length asymmetry. Positive values mean greater right step - # length than left. - stepLengthAsymmetryGoal.setTargetAsymmetry(-0.10) - # Set goal weight - stepLengthAsymmetryGoal.setWeight(5) - # Add the goal to the problem - problem.addGoal(stepLengthAsymmetryGoal) - - - - # ********************************** - # SET BOUNDS - - # Set time bounds - problem.setTimeBounds(0.0, 0.94) - - # Coordinate bounds as dict - coord_bounds = {} - coord_bounds["/jointset/groundPelvis/pelvis_tilt/value"] = [-20*pi/180, - 20*pi/180] - coord_bounds["/jointset/groundPelvis/pelvis_tx/value"] = [0, 2] - coord_bounds["/jointset/groundPelvis/pelvis_ty/value"] = [0.75, 1.25] - coord_bounds["/jointset/hip_r/hip_flexion_r/value"] = [-10*pi/180, - 60*pi/180] - coord_bounds["/jointset/hip_l/hip_flexion_l/value"] = [-10*pi/180, - 60*pi/180] - coord_bounds["/jointset/knee_r/knee_angle_r/value"] = [-50*pi/180, 0] - coord_bounds["/jointset/knee_l/knee_angle_l/value"] = [-50*pi/180, 0] - coord_bounds["/jointset/ankle_r/ankle_angle_r/value"] = [-15*pi/180, - 25*pi/180] - coord_bounds["/jointset/ankle_l/ankle_angle_l/value"] = [-15*pi/180, - 25*pi/180] - coord_bounds["/jointset/lumbar/lumbar/value"] = [0, 20*pi/180] - - # Set coordinate bounds - for bnd in coord_bounds: - problem.setStateInfo(bnd, coord_bounds[bnd]) - - - - # ********************************** - # SOLVE - - # Configure the solver - solver = study.initCasADiSolver() - solver.set_num_mesh_intervals(100) - solver.set_verbosity(2) - solver.set_optim_convergence_tolerance(1e-4) - solver.set_optim_constraint_tolerance(1e-4) - solver.set_optim_max_iterations(2000) - - # Set the initial guess to be the symmetric two-steps tracking solution - # from example2DWalking.py. Run this first, or proceed without a guess. - guess_file = "walk_2D_two_steps_tracking_solution.sto" - if os.path.isfile(guess_file): - solver.setGuessFile(guess_file) - - # Print the Study to file - study.printToXML("example2DWalkingStepLengthAsymmetry.omoco") - - - # Solve - solution = study.solve() - solution.write("walk_2D_step_length_asym_solution.sto") - - # Write predicted GRF to file - contact_r = ["contactHeel_r", "contactFront_r"] - contact_l = ["contactHeel_l", "contactFront_l"] - contact_forces_table = osim.createExternalLoadsTableForGait(model, - solution, contact_r, contact_l) - osim.STOFileAdapter().write(contact_forces_table, - "walk_2D_step_length_asym_ground_forces.sto") - - # Calculate step time asymmetry - _, step_length_asym = computeStepAsymmetry(model, 25, - "walk_2D_step_length_asym_solution.sto", - "walk_2D_step_length_asym_ground_forces.sto") - print("\nStep length asymmetry: %f\n" % step_length_asym) - - - return study, solution - - - -""" -computeStepAsymmetry(): - -Calculate the values of the step length and step time asymmetry from the -results of the simulation. -""" -def computeStepAsymmetry(model_file, threshold, solution_file, grf_file): - - # Load model - model = osim.Model(model_file) - - # Load predicted GRF - grf = np.genfromtxt(grf_file, skip_header=5, delimiter="\t") - - # GRF time vector and vertical components - tvec = grf[:, 0] - - - # ********************************** - # STEP TIME ASYMMETRY - - # Find index of heel strike on each leg - hs_idxR = findHeelStrikeIndex(grf[:, 2], threshold) - hs_idxL = findHeelStrikeIndex(grf[:, 8], threshold) - - # Compute step time on each leg - hs_timeR = tvec[hs_idxR] - hs_timeL = tvec[hs_idxL] - if hs_timeR < hs_timeL: - step_timeL = hs_timeL - hs_timeR - step_timeR = tvec[-1] - step_timeL - else: - step_timeR = hs_timeR - hs_timeL - step_timeL = tvec[-1] - step_timeR - - # Calculate step time asymmetry (%) - step_time_asym = 100 * (step_timeR - step_timeL) \ - / (step_timeR + step_timeL) - - - # ********************************** - # STEP LENGTH ASYMMETRY - - # Get the states for each limb at the instant of heel strike on that limb - states_traj = osim.StatesTrajectory().createFromStatesTable(model, - osim.TimeSeriesTable(solution_file), False, True, True) - statesR = states_traj.get(hs_idxR) - statesL = states_traj.get(hs_idxL) - - # Calculate the step length - step_lengthR = calculateStepLength(model, statesR) - step_lengthL = calculateStepLength(model, statesL) - - # Calculate step length asymmetry (%) - step_length_asym = 100 * (step_lengthR - step_lengthL) \ - / (step_lengthR + step_lengthL) - - - return step_time_asym, step_length_asym - - - -""" -findHeelStrikeIndex(): - -Find heel strike index by determining foot contact on-off instances. If no -heel strike is found, then assume first index is heel strike. This -implementation differs from that of Russell T. Johnson's Matlab version, but -follows the same prinicples. -""" -def findHeelStrikeIndex(grfy, threshold): - - # Find windows representing ground contact - is_contact = (grfy > threshold).astype(int) - - # Calculate transition points, i.e. heel strike (1) and foot off (-1) - contact_diff = np.diff(np.insert(is_contact, 0, 1)) - - # Extract heel strike and foot off indices. If no heel strike found, - # assume first index is heel strike. - idxs = np.where(contact_diff == 1)[0] - if idxs.size == 0: - idx = 0 - else: - idx = idxs[0] - - return int(idx) - - - -""" -calculateStepLength(): - -Find step length by configuring the model at heel strike, then compute distance -between contact spheres along the fore-aft coordinate. -""" -def calculateStepLength(model, state): - - # Configure the model at heel strike - model.initSystem() - model.realizePosition(state) - - # Get the heel contact spheres - contact_r = model.getContactGeometrySet().get("heel_r") - contact_l = model.getContactGeometrySet().get("heel_l") - - # Find the positions of the contact spheres in the global frame - pos_r = contact_r.getFrame().getPositionInGround(state) - pos_l = contact_l.getFrame().getPositionInGround(state) - - # Step length is the difference between global position of the left and - # right along the fore-aft coordinate (x) - step_length = abs(pos_r.get(0) - pos_l.get(0)) - - return step_length - - - - -# %% STEP TIME ASYMMETRY - -# Solve and visualise -step_time_study, step_time_solution = stepTimeAsymmetry() -step_time_study.visualize(step_time_solution) - - - -# %% STEP LENGTH ASYMMETRY - -# Solve and visualise -step_length_study, step_length_solution = stepLengthAsymmetry() -step_length_study.visualize(step_length_solution) - From dedf39b3a8064cf68efcf6ad2805aa2eadcadd1d Mon Sep 17 00:00:00 2001 From: psbiomech Date: Mon, 27 Jun 2022 15:53:42 +1000 Subject: [PATCH 8/8] Updated to 80 chars per line --- .../Moco/example2DWalking/example2DWalking.py | 164 +++++++++++------- .../example2DWalkingMetabolics.py | 39 +++-- .../example2DWalkingStepAsymmetry.py | 74 +++++--- 3 files changed, 174 insertions(+), 103 deletions(-) diff --git a/Bindings/Python/examples/Moco/example2DWalking/example2DWalking.py b/Bindings/Python/examples/Moco/example2DWalking/example2DWalking.py index 182ac5d089..4a788cd379 100644 --- a/Bindings/Python/examples/Moco/example2DWalking/example2DWalking.py +++ b/Bindings/Python/examples/Moco/example2DWalking/example2DWalking.py @@ -63,9 +63,9 @@ def gait_tracking(): track.set_initial_time(0.0) track.set_final_time(0.47008941) - # Call initialize() to receive a pre-configured MocoStudy object based on the - # settings above. Use this to customize the problem beyond the MocoTrack - # interface. + # Call initialize() to receive a pre-configured MocoStudy object based on + # the settings above. Use this to customize the problem beyond the + # Mocotrack interface. study = track.initialize() # Get a writable reference to the MocoProblem from the MocoStudy to perform @@ -78,52 +78,61 @@ def gait_tracking(): # SET GOALS # Symmetry: - # This goal allows us to simulate only one step with left-right symmetry that - # we can then double to create two steps, one on each leg (IFO>IFS>CFO>CFS). - # Note that this is not actually a full gait cycle (IFO>IFS>CFO>CFS>IFO). + # This goal allows us to simulate only one step with left-right symmetry + # that we can then double to create two steps, one on each leg + # (IFO>IFS>CFO>CFS). Note that this is not actually a full gait cycle + # (IFO>IFS>CFO>CFS>IFO). symmetryGoal = osim.MocoPeriodicityGoal("symmetry_goal") problem.addGoal(symmetryGoal) # Enforce periodic symmetry model = modelProcessor.process() model.initSystem() - state_names = [model.getStateVariableNames().getitem(sn) for sn in range(model.getNumStateVariables())] + state_names = [model.getStateVariableNames().getitem(sn) + for sn in range(model.getNumStateVariables())] for sn in state_names: # Symmetric coordinate values and speeds (except for pelvis_tx value): - # Here we constrain final coordinate values of one leg to match the initial - # value of the other leg. Manually add pelvis_tx speed later. + # Here we constrain final coordinate values of one leg to match the + # initial value of the other leg. Manually add pelvis_tx speed later. if "jointset" in sn: if "_r" in sn: - symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, sn.replace("_r", "_l"))) + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, + sn.replace("_r", "_l"))) elif "_l" in sn: - symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, sn.replace("_l", "_r"))) + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, + sn.replace("_l", "_r"))) elif "pelvis_tx" not in sn: symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn)) # Symmetric muscle activations: - # Here, we constrain final muscle activation values of one leg to match the - # initial activation values of the other leg. + # Here, we constrain final muscle activation values of one leg to match + # the initial activation values of the other leg. elif "activation" in sn: if "_r" in sn: - symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, sn.replace("_r", "_l"))) + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, + sn.replace("_r", "_l"))) elif "_l" in sn: - symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, sn.replace("_l", "_r"))) + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, + sn.replace("_l", "_r"))) # The pelvis_tx speed has periodic symmetry - symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair("/jointset/groundPelvis/pelvis_tx/speed")) + symmetryGoal.addStatePair( + osim.MocoPeriodicityGoalPair("/jointset/groundPelvis/pelvis_tx/speed")) # Lumbar control has periodic symmetry. The other controls don't need - # symmetry enforced as they are all muscle excitations. Their behaviour will be - # contstrained by the periodicity imposed on their respective activations. + # symmetry enforced as they are all muscle excitations. Their behaviour + # will be contstrained by the periodicity imposed on their respective + # activations. symmetryGoal.addControlPair(osim.MocoPeriodicityGoalPair('/lumbarAct')) # Effort: # Get a reference to the MocoControlGoal that is added to a MocoTrack # problem by default and adjust the weight - effort = osim.MocoControlGoal.safeDownCast(problem.updGoal("control_effort")) + effort = osim.MocoControlGoal.safeDownCast( + problem.updGoal("control_effort")) effort.setWeight(10.0) @@ -135,7 +144,8 @@ def gait_tracking(): if grf_tracking_weight != 0: # Create a contact tracking goal - contactTracking = osim.MocoContactTrackingGoal("contact", grf_tracking_weight) + contactTracking = osim.MocoContactTrackingGoal("contact", + grf_tracking_weight) contactTracking.setExternalLoadsFile("referenceGRF.xml") # Add contact groups. The sum of all the contact forces in each group @@ -143,7 +153,7 @@ def gait_tracking(): contactTracking.addContactGroup(contact_r, "Right_GRF") contactTracking.addContactGroup(contact_l, "Left_GRF") - # 2D walking problem so consider force errors in the sagittal plane only + # 2D walking problem so consider force errors in the sagittal plane contactTracking.setProjection("plane") contactTracking.setProjectionVector(osim.Vec3(0, 0, 1)) @@ -157,15 +167,20 @@ def gait_tracking(): # Coordinate bounds as dict coord_bounds = {} - coord_bounds["/jointset/groundPelvis/pelvis_tilt/value"] = [-20*pi/180, -10*pi/180] + coord_bounds["/jointset/groundPelvis/pelvis_tilt/value"] = [-20*pi/180, + -10*pi/180] coord_bounds["/jointset/groundPelvis/pelvis_tx/value"] = [0.0, 1.0] coord_bounds["/jointset/groundPelvis/pelvis_ty/value"] = [0.75, 1.25] - coord_bounds["/jointset/hip_r/hip_flexion_r/value"] = [-10*pi/180, 60*pi/180] - coord_bounds["/jointset/hip_l/hip_flexion_l/value"] = [-10*pi/180, 60*pi/180] + coord_bounds["/jointset/hip_r/hip_flexion_r/value"] = [-10*pi/180, + 60*pi/180] + coord_bounds["/jointset/hip_l/hip_flexion_l/value"] = [-10*pi/180, + 60*pi/180] coord_bounds["/jointset/knee_r/knee_angle_r/value"] = [-50*pi/180, 0] coord_bounds["/jointset/knee_l/knee_angle_l/value"] = [-50*pi/180, 0] - coord_bounds["/jointset/ankle_r/ankle_angle_r/value"] = [-15*pi/180, 25*pi/180] - coord_bounds["/jointset/ankle_l/ankle_angle_l/value"] = [-15*pi/180, 25*pi/180] + coord_bounds["/jointset/ankle_r/ankle_angle_r/value"] = [-15*pi/180, + 25*pi/180] + coord_bounds["/jointset/ankle_l/ankle_angle_l/value"] = [-15*pi/180, + 25*pi/180] coord_bounds["/jointset/lumbar/lumbar/value"] = [0, 20*pi/180] # Set coordinate bounds @@ -177,9 +192,9 @@ def gait_tracking(): # ********************************** # SOLVE - # The Solver is pre-configured, however, uncomment below to configure further - # if desired. This gets a writable reference to the Solver, for custom - # configuration. + # The Solver is pre-configured, however, uncomment below to configure + # further if desired. This gets a writable reference to the Solver, for + # custom configuration. # solver = study.updSolver() # solver.set_num_mesh_intervals(50); # solver.set_verbosity(2); @@ -198,10 +213,12 @@ def gait_tracking(): two_steps_solution = osim.createPeriodicTrajectory(solution) two_steps_solution.write("walk_2D_two_steps_tracking_solution.sto") - # Also extract the predicted ground forces, see API documentation for use of - # this utility function - contact_forces_table = osim.createExternalLoadsTableForGait(model, two_steps_solution, contact_r, contact_l) - osim.STOFileAdapter().write(contact_forces_table, "walk_2D_two_steps_tracking_ground_forces.sto") + # Also extract the predicted ground forces, see API documentation for use + # of this utility function + contact_forces_table = osim.createExternalLoadsTableForGait(model, + two_steps_solution, contact_r, contact_l) + osim.STOFileAdapter().write(contact_forces_table, + "walk_2D_two_steps_tracking_ground_forces.sto") return study, solution, two_steps_solution @@ -252,35 +269,41 @@ def gait_prediction(tracking_solution): # Enforce periodic symmetry model = modelProcessor.process() model.initSystem() - state_names = [model.getStateVariableNames().getitem(sn) for sn in range(model.getNumStateVariables())] + state_names = [model.getStateVariableNames().getitem(sn) + for sn in range(model.getNumStateVariables())] for sn in state_names: # Symmetric coordinate values and speeds (except for pelvis_tx value): - # Here we constrain final coordinate values of one leg to match the initial - # value of the other leg. Manually add pelvis_tx speed later. + # Here we constrain final coordinate values of one leg to match the + # initial value of the other leg. Manually add pelvis_tx speed later. if "jointset" in sn: if "_r" in sn: - symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, sn.replace("_r", "_l"))) + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, + sn.replace("_r", "_l"))) elif "_l" in sn: - symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, sn.replace("_l", "_r"))) + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, + sn.replace("_l", "_r"))) elif "pelvis_tx" not in sn: symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn)) # Symmetric muscle activations: - # Here, we constrain final muscle activation values of one leg to match the - # initial activation values of the other leg. + # Here, we constrain final muscle activation values of one leg to match + # the initial activation values of the other leg. elif "activation" in sn: if "_r" in sn: - symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, sn.replace("_r", "_l"))) + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, + sn.replace("_r", "_l"))) elif "_l" in sn: - symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, sn.replace("_l", "_r"))) + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, + sn.replace("_l", "_r"))) # The pelvis_tx speed has periodic symmetry - symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair("/jointset/groundPelvis/pelvis_tx/speed")) + symmetryGoal.addStatePair( + osim.MocoPeriodicityGoalPair("/jointset/groundPelvis/pelvis_tx/speed")) # Lumbar control has periodic symmetry. The other controls don't need - # symmetry enforces as they are all muscle excitations. Their behaviour will be - # modulated by the periodicity imposed on their respective activations. + # symmetry enforces as they are all muscle excitations. Their behaviour + # will be modulated by the periodicity imposed on their activations. symmetryGoal.addControlPair(osim.MocoPeriodicityGoalPair('/lumbarAct')) # Specify the desired average walk speed, add as a goal @@ -288,13 +311,13 @@ def gait_prediction(tracking_solution): speedGoal.set_desired_average_speed(1.2) problem.addGoal(speedGoal) - # Minimise total "effort" over the distance, i.e. minimise sum of the absolute - # values of all controls raised to a given exponent, integrated over the phase. - # In a MocoTrack problem, this is automatically added, however, in this - # predictive problem, we have created a MocoStudy from scratch, so we need to - # add this goal manually to the Problem. The second input argument to the - # constructor is the weight applied to this goal. We also normalise the effort - # by the distance travelled. + # Minimise total "effort" over the distance, i.e. minimise sum of the + # absolute values of all controls raised to a given exponent, integrated + # over the phase. In a MocoTrack problem, this is automatically added, + # however, in this predictive problem, we have created a MocoStudy from + # scratch, so we need to add this goal manually to the Problem. The second + # input argument to the constructor is the weight applied to this goal. + # We also normalise the effort by the distance travelled. effortGoal = osim.MocoControlGoal('effort', 10) effortGoal.setExponent(3) effortGoal.setDivideByDisplacement(True) @@ -311,15 +334,20 @@ def gait_prediction(tracking_solution): # Coordinate bounds as dict coord_bounds = {} - coord_bounds["/jointset/groundPelvis/pelvis_tilt/value"] = [-20*pi/180, -10*pi/180] + coord_bounds["/jointset/groundPelvis/pelvis_tilt/value"] = [-20*pi/180, + -10*pi/180] coord_bounds["/jointset/groundPelvis/pelvis_tx/value"] = [0.0, 1.0] coord_bounds["/jointset/groundPelvis/pelvis_ty/value"] = [0.75, 1.25] - coord_bounds["/jointset/hip_r/hip_flexion_r/value"] = [-10*pi/180, 60*pi/180] - coord_bounds["/jointset/hip_l/hip_flexion_l/value"] = [-10*pi/180, 60*pi/180] + coord_bounds["/jointset/hip_r/hip_flexion_r/value"] = [-10*pi/180, + 60*pi/180] + coord_bounds["/jointset/hip_l/hip_flexion_l/value"] = [-10*pi/180, + 60*pi/180] coord_bounds["/jointset/knee_r/knee_angle_r/value"] = [-50*pi/180, 0] coord_bounds["/jointset/knee_l/knee_angle_l/value"] = [-50*pi/180, 0] - coord_bounds["/jointset/ankle_r/ankle_angle_r/value"] = [-15*pi/180, 25*pi/180] - coord_bounds["/jointset/ankle_l/ankle_angle_l/value"] = [-15*pi/180, 25*pi/180] + coord_bounds["/jointset/ankle_r/ankle_angle_r/value"] = [-15*pi/180, + 25*pi/180] + coord_bounds["/jointset/ankle_l/ankle_angle_l/value"] = [-15*pi/180, + 25*pi/180] coord_bounds["/jointset/lumbar/lumbar/value"] = [0, 20*pi/180] # Set coordinate bounds @@ -331,9 +359,9 @@ def gait_prediction(tracking_solution): # ********************************** # SOLVE - # Configure the solver. In the tracking problem, the solver was pre-configured, - # using MocoTrack, however, as we have created a MocoStudy from scratch, we - # need to initialise the MocoSolver and configure it. + # Configure the solver. In the tracking problem, the solver was + # pre-configured, using MocoTrack, however, as we have created a MocoStudy + # from scratch, we need to initialise the MocoSolver and configure it. solver = study.initCasADiSolver() solver.set_num_mesh_intervals(50) solver.set_verbosity(2) @@ -355,12 +383,14 @@ def gait_prediction(tracking_solution): two_steps_solution = osim.createPeriodicTrajectory(solution) two_steps_solution.write("walk_2D_two_steps_prediction_solution.sto") - # Also extract the predicted ground forces, see API documentation for use of - # this utility function + # Also extract the predicted ground forces, see API documentation for use + # of this utility function contact_r = ["contactHeel_r", "contactFront_r"] contact_l = ["contactHeel_l", "contactFront_l"] - contact_forces_table = osim.createExternalLoadsTableForGait(model, two_steps_solution, contact_r, contact_l) - osim.STOFileAdapter().write(contact_forces_table, "walk_2D_two_steps_prediction_ground_forces.sto") + contact_forces_table = osim.createExternalLoadsTableForGait(model, + two_steps_solution, contact_r, contact_l) + osim.STOFileAdapter().write(contact_forces_table, + "walk_2D_two_steps_prediction_ground_forces.sto") return study, solution, two_steps_solution @@ -370,7 +400,8 @@ def gait_prediction(tracking_solution): # %% TRACKING PROBLEM # Solve the problem and visualise -tracking_study, tracking_solution, tracking_two_steps_solution = gait_tracking() +tracking_study, tracking_solution, tracking_two_steps_solution = \ + gait_tracking() tracking_study.visualize(tracking_two_steps_solution) @@ -378,7 +409,8 @@ def gait_prediction(tracking_solution): # %% PREDICTION PROBLEM # Solve the problem and visualise (uses tracking_solution as initial guess) -prediction_study, prediction_solution, prediction_two_steps_solution = gait_prediction(tracking_solution) +prediction_study, prediction_solution, prediction_two_steps_solution = \ + gait_prediction(tracking_solution) prediction_study.visualize(prediction_two_steps_solution) diff --git a/Bindings/Python/examples/Moco/example2DWalking/example2DWalkingMetabolics.py b/Bindings/Python/examples/Moco/example2DWalking/example2DWalkingMetabolics.py index 13c6fad8d6..88b36600cf 100644 --- a/Bindings/Python/examples/Moco/example2DWalking/example2DWalkingMetabolics.py +++ b/Bindings/Python/examples/Moco/example2DWalking/example2DWalkingMetabolics.py @@ -95,7 +95,8 @@ # Enforce periodic symmetry model = modelProcessor.process() model.initSystem() -state_names = [model.getStateVariableNames().getitem(sn) for sn in range(model.getNumStateVariables())] +state_names = [model.getStateVariableNames().getitem(sn) + for sn in range(model.getNumStateVariables())] for sn in state_names: # Symmetric coordinate values and speeds (except for pelvis_tx value): @@ -103,9 +104,11 @@ # value of the other leg. Manually add pelvis_tx speed later. if "jointset" in sn: if "_r" in sn: - symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, sn.replace("_r", "_l"))) + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, + sn.replace("_r", "_l"))) elif "_l" in sn: - symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, sn.replace("_l", "_r"))) + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, + sn.replace("_l", "_r"))) elif "pelvis_tx" not in sn: symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn)) @@ -114,13 +117,16 @@ # initial activation values of the other leg. elif "activation" in sn: if "_r" in sn: - symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, sn.replace("_r", "_l"))) + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, + sn.replace("_r", "_l"))) elif "_l" in sn: - symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, sn.replace("_l", "_r"))) + symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn, + sn.replace("_l", "_r"))) # The pelvis_tx speed has periodic symmetry -symmetryGoal.addStatePair(osim.MocoPeriodicityGoalPair("/jointset/groundPelvis/pelvis_tx/speed")) +symmetryGoal.addStatePair( + osim.MocoPeriodicityGoalPair("/jointset/groundPelvis/pelvis_tx/speed")) # Lumbar control has periodic symmetry. The other controls don't need symmetry # enforced as they are all muscle excitations. Their behaviour will be @@ -151,15 +157,20 @@ # Coordinate bounds as dict coord_bounds = {} -coord_bounds["/jointset/groundPelvis/pelvis_tilt/value"] = [-20*pi/180, -10*pi/180] +coord_bounds["/jointset/groundPelvis/pelvis_tilt/value"] = [-20*pi/180, + -10*pi/180] coord_bounds["/jointset/groundPelvis/pelvis_tx/value"] = [0.0, 1.0] coord_bounds["/jointset/groundPelvis/pelvis_ty/value"] = [0.75, 1.25] -coord_bounds["/jointset/hip_r/hip_flexion_r/value"] = [-10*pi/180, 60*pi/180] -coord_bounds["/jointset/hip_l/hip_flexion_l/value"] = [-10*pi/180, 60*pi/180] +coord_bounds["/jointset/hip_r/hip_flexion_r/value"] = [-10*pi/180, + 60*pi/180] +coord_bounds["/jointset/hip_l/hip_flexion_l/value"] = [-10*pi/180, + 60*pi/180] coord_bounds["/jointset/knee_r/knee_angle_r/value"] = [-50*pi/180, 0] coord_bounds["/jointset/knee_l/knee_angle_l/value"] = [-50*pi/180, 0] -coord_bounds["/jointset/ankle_r/ankle_angle_r/value"] = [-15*pi/180, 25*pi/180] -coord_bounds["/jointset/ankle_l/ankle_angle_l/value"] = [-15*pi/180, 25*pi/180] +coord_bounds["/jointset/ankle_r/ankle_angle_r/value"] = [-15*pi/180, + 25*pi/180] +coord_bounds["/jointset/ankle_l/ankle_angle_l/value"] = [-15*pi/180, + 25*pi/180] coord_bounds["/jointset/lumbar/lumbar/value"] = [0, 20*pi/180] # Set coordinate bounds @@ -192,8 +203,10 @@ # this utility function contact_r = ["contactHeel_r", "contactFront_r"] contact_l = ["contactHeel_l", "contactFront_l"] -contact_forces_table = osim.createExternalLoadsTableForGait(model, two_steps_solution, contact_r, contact_l) -osim.STOFileAdapter().write(contact_forces_table, "walk_2D_metabolics_two_steps_tracking_ground_forces.sto") +contact_forces_table = osim.createExternalLoadsTableForGait(model, + two_steps_solution, contact_r, contact_l) +osim.STOFileAdapter().write(contact_forces_table, + "walk_2D_metabolics_two_steps_tracking_ground_forces.sto") # Determine the cost of transport from the metabolics cost term. Need to divide diff --git a/Bindings/Python/examples/Moco/example2DWalking/example2DWalkingStepAsymmetry.py b/Bindings/Python/examples/Moco/example2DWalking/example2DWalkingStepAsymmetry.py index 502f51c813..fd8fdfdefe 100644 --- a/Bindings/Python/examples/Moco/example2DWalking/example2DWalkingStepAsymmetry.py +++ b/Bindings/Python/examples/Moco/example2DWalking/example2DWalkingStepAsymmetry.py @@ -77,7 +77,8 @@ def stepTimeAsymmetry(): # Get the model modelProcessor = osim.ModelProcessor("2D_gait.osim") - modelProcessor.append(osim.ModOpTendonComplianceDynamicsModeDGF("implicit")) + modelProcessor.append( + osim.ModOpTendonComplianceDynamicsModeDGF("implicit")) # Get the MocoProblem from the MocoStudy and set the model on it problem = study.updProblem() @@ -94,7 +95,8 @@ def stepTimeAsymmetry(): periodicityGoal = osim.MocoPeriodicityGoal("periodicity") model = modelProcessor.process() model.initSystem() - state_names = [model.getStateVariableNames().getitem(sn) for sn in range(model.getNumStateVariables())] + state_names = [model.getStateVariableNames().getitem(sn) + for sn in range(model.getNumStateVariables())] for sn in state_names: if "pelvis_tx/value" not in sn: periodicityGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn)) @@ -114,7 +116,8 @@ def stepTimeAsymmetry(): # Step time asymmetry: # Create the goal, and configure it - stepTimeAsymmetryGoal = osim.MocoStepTimeAsymmetryGoal("step_time_asymmetry") + stepTimeAsymmetryGoal = \ + osim.MocoStepTimeAsymmetryGoal("step_time_asymmetry") # Value for smoothing term used to compute when foot contact is made # (default = 0.25). Users may need to adjust this based on convergence and # matching the target asymmetry. @@ -149,15 +152,20 @@ def stepTimeAsymmetry(): # Coordinate bounds as dict coord_bounds = {} - coord_bounds["/jointset/groundPelvis/pelvis_tilt/value"] = [-20*pi/180, 20*pi/180] + coord_bounds["/jointset/groundPelvis/pelvis_tilt/value"] = [-20*pi/180, + 20*pi/180] coord_bounds["/jointset/groundPelvis/pelvis_tx/value"] = [0, 2] coord_bounds["/jointset/groundPelvis/pelvis_ty/value"] = [0.75, 1.25] - coord_bounds["/jointset/hip_r/hip_flexion_r/value"] = [-10*pi/180, 60*pi/180] - coord_bounds["/jointset/hip_l/hip_flexion_l/value"] = [-10*pi/180, 60*pi/180] + coord_bounds["/jointset/hip_r/hip_flexion_r/value"] = [-10*pi/180, + 60*pi/180] + coord_bounds["/jointset/hip_l/hip_flexion_l/value"] = [-10*pi/180, + 60*pi/180] coord_bounds["/jointset/knee_r/knee_angle_r/value"] = [-50*pi/180, 0] coord_bounds["/jointset/knee_l/knee_angle_l/value"] = [-50*pi/180, 0] - coord_bounds["/jointset/ankle_r/ankle_angle_r/value"] = [-15*pi/180, 25*pi/180] - coord_bounds["/jointset/ankle_l/ankle_angle_l/value"] = [-15*pi/180, 25*pi/180] + coord_bounds["/jointset/ankle_r/ankle_angle_r/value"] = [-15*pi/180, + 25*pi/180] + coord_bounds["/jointset/ankle_l/ankle_angle_l/value"] = [-15*pi/180, + 25*pi/180] coord_bounds["/jointset/lumbar/lumbar/value"] = [0, 20*pi/180] # Set coordinate bounds @@ -193,11 +201,15 @@ def stepTimeAsymmetry(): # Write predicted GRF to file - contact_forces_table = osim.createExternalLoadsTableForGait(model, solution, contact_r, contact_l) - osim.STOFileAdapter().write(contact_forces_table, "walk_2D_step_time_asym_ground_forces.sto") + contact_forces_table = osim.createExternalLoadsTableForGait(model, + solution, contact_r, contact_l) + osim.STOFileAdapter().write(contact_forces_table, + "walk_2D_step_time_asym_ground_forces.sto") # Calculate step time asymmetry - step_time_asym, _ = computeStepAsymmetry(model, 25, "walk_2D_step_time_asym_solution.sto", "walk_2D_step_time_asym_ground_forces.sto") + step_time_asym, _ = computeStepAsymmetry(model, 25, + "walk_2D_step_time_asym_solution.sto", + "walk_2D_step_time_asym_ground_forces.sto") print("\nStep time asymmetry: %f\n" % step_time_asym) @@ -257,7 +269,8 @@ def stepLengthAsymmetry(): # Get the model modelProcessor = osim.ModelProcessor("2D_gait.osim") - modelProcessor.append(osim.ModOpTendonComplianceDynamicsModeDGF("implicit")) + modelProcessor.append( + osim.ModOpTendonComplianceDynamicsModeDGF("implicit")) # Get the MocoProblem from the MocoStudy and set the model on it problem = study.updProblem() @@ -274,7 +287,8 @@ def stepLengthAsymmetry(): periodicityGoal = osim.MocoPeriodicityGoal("periodicity") model = modelProcessor.process() model.initSystem() - state_names = [model.getStateVariableNames().getitem(sn) for sn in range(model.getNumStateVariables())] + state_names = [model.getStateVariableNames().getitem(sn) + for sn in range(model.getNumStateVariables())] for sn in state_names: if "pelvis_tx/value" not in sn: periodicityGoal.addStatePair(osim.MocoPeriodicityGoalPair(sn)) @@ -322,15 +336,20 @@ def stepLengthAsymmetry(): # Coordinate bounds as dict coord_bounds = {} - coord_bounds["/jointset/groundPelvis/pelvis_tilt/value"] = [-20*pi/180, 20*pi/180] + coord_bounds["/jointset/groundPelvis/pelvis_tilt/value"] = [-20*pi/180, + 20*pi/180] coord_bounds["/jointset/groundPelvis/pelvis_tx/value"] = [0, 2] coord_bounds["/jointset/groundPelvis/pelvis_ty/value"] = [0.75, 1.25] - coord_bounds["/jointset/hip_r/hip_flexion_r/value"] = [-10*pi/180, 60*pi/180] - coord_bounds["/jointset/hip_l/hip_flexion_l/value"] = [-10*pi/180, 60*pi/180] + coord_bounds["/jointset/hip_r/hip_flexion_r/value"] = [-10*pi/180, + 60*pi/180] + coord_bounds["/jointset/hip_l/hip_flexion_l/value"] = [-10*pi/180, + 60*pi/180] coord_bounds["/jointset/knee_r/knee_angle_r/value"] = [-50*pi/180, 0] coord_bounds["/jointset/knee_l/knee_angle_l/value"] = [-50*pi/180, 0] - coord_bounds["/jointset/ankle_r/ankle_angle_r/value"] = [-15*pi/180, 25*pi/180] - coord_bounds["/jointset/ankle_l/ankle_angle_l/value"] = [-15*pi/180, 25*pi/180] + coord_bounds["/jointset/ankle_r/ankle_angle_r/value"] = [-15*pi/180, + 25*pi/180] + coord_bounds["/jointset/ankle_l/ankle_angle_l/value"] = [-15*pi/180, + 25*pi/180] coord_bounds["/jointset/lumbar/lumbar/value"] = [0, 20*pi/180] # Set coordinate bounds @@ -367,11 +386,15 @@ def stepLengthAsymmetry(): # Write predicted GRF to file contact_r = ["contactHeel_r", "contactFront_r"] contact_l = ["contactHeel_l", "contactFront_l"] - contact_forces_table = osim.createExternalLoadsTableForGait(model, solution, contact_r, contact_l) - osim.STOFileAdapter().write(contact_forces_table, "walk_2D_step_length_asym_ground_forces.sto") + contact_forces_table = osim.createExternalLoadsTableForGait(model, + solution, contact_r, contact_l) + osim.STOFileAdapter().write(contact_forces_table, + "walk_2D_step_length_asym_ground_forces.sto") # Calculate step time asymmetry - _, step_length_asym = computeStepAsymmetry(model, 25, "walk_2D_step_length_asym_solution.sto", "walk_2D_step_length_asym_ground_forces.sto") + _, step_length_asym = computeStepAsymmetry(model, 25, + "walk_2D_step_length_asym_solution.sto", + "walk_2D_step_length_asym_ground_forces.sto") print("\nStep length asymmetry: %f\n" % step_length_asym) @@ -415,14 +438,16 @@ def computeStepAsymmetry(model_file, threshold, solution_file, grf_file): step_timeL = tvec[-1] - step_timeR # Calculate step time asymmetry (%) - step_time_asym = 100 * (step_timeR - step_timeL) / (step_timeR + step_timeL) + step_time_asym = 100 * (step_timeR - step_timeL) \ + / (step_timeR + step_timeL) # ********************************** # STEP LENGTH ASYMMETRY # Get the states for each limb at the instant of heel strike on that limb - states_traj = osim.StatesTrajectory().createFromStatesTable(model, osim.TimeSeriesTable(solution_file), False, True, True) + states_traj = osim.StatesTrajectory().createFromStatesTable(model, + osim.TimeSeriesTable(solution_file), False, True, True) statesR = states_traj.get(hs_idxR) statesL = states_traj.get(hs_idxL) @@ -431,7 +456,8 @@ def computeStepAsymmetry(model_file, threshold, solution_file, grf_file): step_lengthL = calculateStepLength(model, statesL) # Calculate step length asymmetry (%) - step_length_asym = 100 * (step_lengthR - step_lengthL) / (step_lengthR + step_lengthL) + step_length_asym = 100 * (step_lengthR - step_lengthL) \ + / (step_lengthR + step_lengthL) return step_time_asym, step_length_asym