Merge remote-tracking branch 'upstream/main' into gepmetry_python_setup

[no ci]

.gitignore

@@ -56,6 +56,7 @@ delete_this_to_stop_optimization*.txt
 .idea/codeStyles/codeStyleConfig.xml
 .idea/codeStyles/Project.xml
 .idea/editor.xml
+.idea/copilot/**
 .cache/clangd/**
 **/.vscode/
 
@@ -71,6 +72,8 @@ delete_this_to_stop_optimization*.txt
 
 .idea/deployment.xml
 .idea/other.xml
+.idea/encodings.xml
+.idea/inspectionProfiles/Project_Default.xml
 
 # Clang-Tidy
 .clang-tidy
@@ -80,4 +83,3 @@ cmake-build-*
 
 ## File-based project format:
 *.iws
-.vscode/settings.json

Bindings/Java/Matlab/examples/Moco/example3DWalking/exampleMocoTrack.m

@@ -324,19 +324,15 @@ function muscleDrivenJointMomentTracking()
 % Ignore coordinates that are locked, prescribed, or coupled to other
 % coordinates via CoordinateCouplerConstraints (true by default).
 jointMomentTracking.setIgnoreConstrainedCoordinates(true);
-coordinateSet = model.getCoordinateSet();
-for i = 0:coordinateSet.getSize()-1
-    coordinate = coordinateSet.get(i);
-    coordName = coordinate.getName();
-    % Don't track generalized forces associated with pelvis residuals.
-    if contains(string(coordName), "pelvis")
-        jointMomentTracking.setWeightForCoordinate(coordName, 0);
-    end
-    % Encourage better tracking of the ankle joint moments.
-    if contains(string(coordName), "ankle")
-        jointMomentTracking.setWeightForCoordinate(coordName, 100);
-    end
-end
+
+% Do not track generalized forces associated with pelvis residuals.
+jointMomentTracking.setWeightForGeneralizedForcePattern(".*pelvis.*", 0);
+
+% Encourage better tracking of the ankle joint moments.
+jointMomentTracking.setWeightForGeneralizedForce("ankle_angle_r_moment", 100);
+jointMomentTracking.setWeightForGeneralizedForce("ankle_angle_l_moment", 100);
+
+% Add the joint moment tracking goal to the problem.
 problem.addGoal(jointMomentTracking);
 
 % Update the solver problem and tolerances.

Bindings/Java/Matlab/tests/testConnectorsInputsOutputs.m

@@ -13,8 +13,8 @@
 source = TableSource();
 source.setName('source');
 table = TimeSeriesTable();
-labels = StdVectorString(4);
-labels.set(0, 'c1'); labels.set(1, 'c2'); labels.set(2, 'c3'); labels.set(3, 'c4');
+labels = StdVectorString();
+labels.add('c1'); labels.add('c2'); labels.add('c3'); labels.add('c4');
 table.setColumnLabels(labels);
 row = RowVector(4, 1.0);
 table.appendRow(0.0, row);

Bindings/Java/Matlab/tests/testOsimC3D.m

@@ -48,8 +48,8 @@
 % col numbers.
 
 % Test Marker Rotations
-randomRows = randsample(markers.getNumRows(),10)-1;
-randomCols = randsample(markers.getNumColumns(),10)-1;
+randomRows = randi(markers.getNumRows(),10)-1;
+randomCols = randi(markers.getNumColumns(),10)-1;
 for i = 1 : 10
     % Get the values in the original and rotated marker tables
     loc = osimVec3ToArray(markers.getRowAtIndex(randomRows(i)).get(randomCols(i)));
@@ -68,8 +68,8 @@
 end
 
 % Test Force Rotations
-randomRows = randsample(forces.getNumRows(),6)-1;
-randomCols = randsample(forces.getNumColumns(),6)-1;
+randomRows = randi(forces.getNumRows(),6)-1;
+randomCols = randi(forces.getNumColumns(),6)-1;
 for i = 1 : 6
     % Get the values in the original and rotated marker tables
     val = osimVec3ToArray(forces.getRowAtIndex(randomRows(i)).get(randomCols(i)));
@@ -93,7 +93,7 @@
 
 % Forces should remain unchanged, point and moments should be in meters
 % (divided by 1000)
-randomRows = randsample(forces.getNumRows(),101)-1;
+randomRows = randi(forces.getNumRows(),101)-1;
 for i =  0 : forces_m.getNumColumns() - 1
 
     col =  forces_rotated.getDependentColumnAtIndex(i);
@@ -106,17 +106,18 @@
             d   = col.get(randomRows(u));
             d_m = col_m.get(randomRows(u));
 
-            assert(d.get(0)==d_m.get(0),'Force Data has been incorrectly altered');
-            assert(d.get(1)==d_m.get(1),'Force Data has been incorrectly altered')
-            assert(d.get(2)==d_m.get(2),'Force Data has been incorrectly altered')
+            assert(abs(d.get(0)-d_m.get(0))<.001,'Force Data has been incorrectly altered');
+            assert(abs(d.get(1)-d_m.get(1))<.001,'Force Data has been incorrectly altered');
+            assert(abs(d.get(2)-d_m.get(2))<.001,'Force Data has been incorrectly altered');
+
         else
             % Get the values
             d   = col.get(randomRows(u));
             d_m = col_m.get(randomRows(u));
-
-            assert(d.get(0)/1000==d_m.get(0),'Point or Moment Data has been incorrectly altered');
-            assert(d.get(1)/1000==d_m.get(1),'Point or Moment Data has been incorrectly altered');
-            assert(d.get(2)/1000==d_m.get(2),'Point or Moment Data has been incorrectly altered');
+            assert(abs(d.get(0)/1000-d_m.get(0))<.001,'Point or Moment Data has been incorrectly altered');
+            assert(abs(d.get(1)/1000-d_m.get(1))<.001,'Point or Moment Data has been incorrectly altered');
+            assert(abs(d.get(2)/1000-d_m.get(2))<.001,'Point or Moment Data has been incorrectly altered');
+
         end
     end
 end

Bindings/Java/tests/TestMocoSlidingMass.java

@@ -82,7 +82,7 @@ public static void testMocoSlidingMass() throws Exception {
     // =====================
     if (MocoCasADiSolver.isAvailable()) {
       MocoCasADiSolver ms = study.initCasADiSolver();
-      ms.set_num_mesh_intervals(100);
+      ms.set_num_mesh_intervals(50);
 
       // Now that we've finished setting up the tool, print it to a file.
       study.print("sliding_mass.omoco");

Bindings/Java/tests/TestTables.java

@@ -265,6 +265,36 @@ public static void test_DataTable() {
         assert tableFlat.getNumRows()             == 3;
         assert tableFlat.getNumColumns()          == 12;
         System.out.println(tableFlat);
+        table = new DataTable();
+        labels = new StdVectorString();
+        labels.add("col0.0"); labels.add("col0.1"); labels.add("col0.2");
+        labels.add("col0.3"); labels.add("col0.4"); labels.add("col0.5");
+        labels.add("col0.6"); labels.add("col0.7"); labels.add("col0.8");
+        labels.add("col1.0"); labels.add("col1.1"); labels.add("col1.2");
+        labels.add("col1.3"); labels.add("col1.4"); labels.add("col1.5");
+        labels.add("col1.6"); labels.add("col1.7"); labels.add("col1.8");
+        table.setColumnLabels(labels);
+        row = new RowVector(18, 1);
+        table.appendRow(1, row);
+        row = new RowVector(18, 2);
+        table.appendRow(2, row);
+        row = new RowVector(18, 3);
+        table.appendRow(3, row);
+        assert table.getColumnLabels().size() == 18;
+        assert table.getNumRows()             == 3;
+        assert table.getNumColumns()          == 18;
+        DataTableRotation tableRot = table.packRotation();
+        assert tableRot.getColumnLabel(0).equals("col0");
+        assert tableRot.getNumRows()    == 3;
+        assert tableRot.getNumColumns() == 2;
+        System.out.println(tableRot);
+        tableFlat = tableRot.flatten();
+        assert tableFlat.getColumnLabels().size() == 18;
+        assert tableFlat.getColumnLabel( 0).equals("col0_1");
+        assert tableFlat.getColumnLabel(15).equals("col1_7");
+        assert tableFlat.getNumRows()             == 3;
+        assert tableFlat.getNumColumns()          == 18;
+        System.out.println(tableFlat);
     }
 
     public static void test_DataTableVec3() {

Bindings/OpenSimHeaders_common.h

@@ -22,6 +22,7 @@
 #include <OpenSim/Common/Event.h>
 #include <OpenSim/Common/Exception.h>
 #include <OpenSim/Common/ExperimentalSensor.h>
+#include <OpenSim/Common/ExpressionBasedFunction.h>
 #include <OpenSim/Common/FileAdapter.h>
 #include <OpenSim/Common/Function.h>
 #include <OpenSim/Common/FunctionSet.h>

Bindings/OpenSimHeaders_simulation.h

@@ -19,6 +19,7 @@
 #include <OpenSim/Simulation/Model/PhysicalOffsetFrame.h>
 
 #include <OpenSim/Simulation/Model/Force.h>
+#include <OpenSim/Simulation/Model/ForceProducer.h>
 #include <OpenSim/Simulation/Model/PrescribedForce.h>
 #include <OpenSim/Simulation/Model/CoordinateLimitForce.h>
 #include <OpenSim/Simulation/Model/ExternalForce.h>
@@ -147,6 +148,7 @@
 #include <OpenSim/Simulation/OpenSense/IMU.h>
 #include <OpenSim/Simulation/OpenSense/OpenSenseUtilities.h>
 
+#include <OpenSim/Simulation/StatesDocument.h>
 #include <OpenSim/Simulation/StatesTrajectory.h>
 #include <OpenSim/Simulation/StatesTrajectoryReporter.h>
 

Bindings/Python/examples/Moco/example3DWalking/exampleMocoTrack.py

@@ -279,18 +279,15 @@ def muscleDrivenJointMomentTracking():
     # Ignore coordinates that are locked, prescribed, or coupled to other
     # coordinates via CoordinateCouplerConstraints (true by default).
     jointMomentTracking.setIgnoreConstrainedCoordinates(True)
-    coordinateSet = model.getCoordinateSet()
-    for i in range(coordinateSet.getSize()):
-        coordinate = coordinateSet.get(i)
-        coordName = coordinate.getName()
-        # Don't track generalized forces associated with pelvis residuals.
-        if 'pelvis' in coordName:
-            jointMomentTracking.setWeightForCoordinate(coordName, 0)
-
-        # Encourage better tracking of the ankle joint moments.
-        if 'ankle' in coordName:
-            jointMomentTracking.setWeightForCoordinate(coordName, 100)
-
+
+    # Do not track generalized forces associated with pelvis residuals.
+    jointMomentTracking.setWeightForGeneralizedForcePattern('.*pelvis.*', 0)
+
+    # Encourage better tracking of the ankle joint moments.
+    jointMomentTracking.setWeightForGeneralizedForce('ankle_angle_r_moment', 100)
+    jointMomentTracking.setWeightForGeneralizedForce('ankle_angle_l_moment', 100)
+
+    # Add the joint moment tracking goal to the problem.
     problem.addGoal(jointMomentTracking)
 
     # Update the solver tolerances.

Bindings/Python/tests/test_DataTable.py

@@ -130,7 +130,7 @@ def test_DataTable(self):
                 row[1] == 200 and
                 row[2] == 300 and
                 row[3] == 400)
-        row0 = table.getRowAtIndex(0)
+        row0 = table.updRowAtIndex(0)
         row0[0] = 10
         row0[1] = 10
         row0[2] = 10
@@ -140,7 +140,7 @@ def test_DataTable(self):
                 row0[1] == 10 and
                 row0[2] == 10 and
                 row0[3] == 10)
-        row2 = table.getRow(0.3)
+        row2 = table.updRow(0.3)
         row2[0] = 20
         row2[1] = 20
         row2[2] = 20
@@ -152,15 +152,15 @@ def test_DataTable(self):
                 row2[3] == 20)
         print(table)
         # Edit columns of the table.
-        col1 = table.getDependentColumnAtIndex(1)
+        col1 = table.updDependentColumnAtIndex(1)
         col1[0] = 30
         col1[1] = 30
         col1[2] = 30
         col1 = table.getDependentColumnAtIndex(1)
         assert (col1[0] == 30 and
                 col1[1] == 30 and
                 col1[2] == 30)
-        col3 = table.getDependentColumn('3')
+        col3 = table.updDependentColumn('3')
         col3[0] = 40
         col3[1] = 40
         col3[2] = 40
@@ -299,6 +299,40 @@ def test_DataTable(self):
         assert tableFlat.getNumRows()           == 3
         assert tableFlat.getNumColumns()        == 12
         print(tableFlat)
+        table = osim.DataTable()
+        table.setColumnLabels(('col0_x', 'col0_y', 'col0_z',
+                               'col1_x', 'col1_y', 'col1_z',
+                               'col2_x', 'col2_y', 'col2_z',
+                               'col3_x', 'col3_y', 'col3_z',
+                               'col4_x', 'col4_y', 'col4_z',
+                               'col5_x', 'col5_y', 'col5_z'))
+        row = osim.RowVector([1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1])
+        table.appendRow(1, row)
+        row = osim.RowVector([2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2, 2])
+        table.appendRow(2, row)
+        row = osim.RowVector([3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3, 3])
+        table.appendRow(3, row)
+        assert len(table.getColumnLabels()) == 18
+        assert table.getNumRows()           == 3
+        assert table.getNumColumns()        == 18
+        table.setColumnLabels(('col0_0', 'col0_1', 'col0_2',
+                               'col0_3', 'col0_4', 'col0_5',
+                               'col0_6', 'col0_7', 'col0_8',
+                               'col1_0', 'col1_1', 'col1_2',
+                               'col1_3', 'col1_4', 'col1_5',
+                               'col1_6', 'col1_7', 'col1_8'))
+        tableRot = table.packRotation()
+        tableRot.getColumnLabels() == ('col0', 'col1')
+        tableRot.getNumRows()    == 3
+        tableRot.getNumColumns() == 2
+        print(tableRot)
+        tableFlat = tableRot.flatten()
+        assert len(tableFlat.getColumnLabels()) == 18
+        assert tableFlat.getColumnLabel( 0) == 'col0_1'
+        assert tableFlat.getColumnLabel(15) == 'col1_7'
+        assert tableFlat.getNumRows()           == 3
+        assert tableFlat.getNumColumns()        == 18
+        print(tableFlat)
 
     def test_TimeSeriesTable(self):
         print()
@@ -475,15 +509,15 @@ def test_DataTableVec3(self):
         print(tableDouble)
 
         # Edit rows of the table.
-        row0 = table.getRowAtIndex(0)
+        row0 = table.updRowAtIndex(0)
         row0[0] = osim.Vec3(10, 10, 10)
         row0[1] = osim.Vec3(10, 10, 10)
         row0[2] = osim.Vec3(10, 10, 10)
         row0 = table.getRowAtIndex(0)
         assert (str(row0[0]) == str(osim.Vec3(10, 10, 10)) and
                 str(row0[1]) == str(osim.Vec3(10, 10, 10)) and
                 str(row0[2]) == str(osim.Vec3(10, 10, 10)))
-        row2 = table.getRow(0.3)
+        row2 = table.updRow(0.3)
         row2[0] = osim.Vec3(20, 20, 20)
         row2[1] = osim.Vec3(20, 20, 20)
         row2[2] = osim.Vec3(20, 20, 20)
@@ -493,15 +527,15 @@ def test_DataTableVec3(self):
                 str(row2[2]) == str(osim.Vec3(20, 20, 20)))
         print(table)
         # Edit columns of the table.
-        col1 = table.getDependentColumnAtIndex(1)
+        col1 = table.updDependentColumnAtIndex(1)
         col1[0] = osim.Vec3(30, 30, 30)
         col1[1] = osim.Vec3(30, 30, 30)
         col1[2] = osim.Vec3(30, 30, 30)
         col1 = table.getDependentColumnAtIndex(1)
         assert (str(col1[0]) == str(osim.Vec3(30, 30, 30)) and
                 str(col1[1]) == str(osim.Vec3(30, 30, 30)) and
                 str(col1[2]) == str(osim.Vec3(30, 30, 30)))
-        col2 = table.getDependentColumn('2')
+        col2 = table.updDependentColumn('2')
         col2[0] = osim.Vec3(40, 40, 40)
         col2[1] = osim.Vec3(40, 40, 40)
         col2[2] = osim.Vec3(40, 40, 40)

Bindings/Python/tests/test_moco.py

@@ -35,6 +35,27 @@ def createSlidingMassModel():
 
     return model
 
+def createDoubleSlidingMassModel():
+    model = createSlidingMassModel()
+    body = osim.Body("body2", 10.0, osim.Vec3(0), osim.Inertia(0))
+    model.addComponent(body)
+
+    joint = osim.SliderJoint("slider2", model.getGround(), body)
+    coord = joint.updCoordinate(osim.SliderJoint.Coord_TranslationX)
+    coord.setName("position");
+    model.addComponent(joint);
+
+    actu = osim.CoordinateActuator()
+    actu.setCoordinate(coord)
+    actu.setName("actuator2")
+    actu.setOptimalForce(1)
+    model.addComponent(actu)
+
+    model.finalizeConnections()
+    model.initSystem()
+    return model
+
+
 class TestSwigAddtlInterface(unittest.TestCase):
     def test_bounds(self):
         model = osim.Model()
@@ -391,3 +412,33 @@ def test_changing_costs(self):
             # Change the weights of the costs.
             effort.setWeight(0.1)
             assert(study.solve().getFinalTime() < 0.8 * finalTime0)
+
+    def test_expression_based_parameter_goal(self):
+        study = osim.MocoStudy()
+        mp = study.updProblem()
+        mp.setModel(createDoubleSlidingMassModel())
+        mp.setTimeBounds(0, 1)
+        mp.setStateInfo("/slider/position/value", [-5, 5], 0, [0.2, 0.3])
+        mp.setStateInfo("/slider/position/speed", [-20, 20])
+        mp.setStateInfo("/slider2/position/value", [-5, 5], 1, [1.2, 1.3])
+        mp.setStateInfo("/slider2/position/speed", [-20, 20])
+
+        parameter = osim.MocoParameter("sphere_mass", "body", "mass",
+                                    osim.MocoBounds(0, 10))
+        mp.addParameter(parameter)
+        parameter2 = osim.MocoParameter("sphere2_mass", "body2", "mass",
+                                     osim.MocoBounds(0, 10))
+        mp.addParameter(parameter2)
+        total_weight = 7
+        mass_goal = osim.MocoExpressionBasedParameterGoal()
+        mp.addGoal(mass_goal)
+        mass_goal.setExpression(f"(p+q-{total_weight})^2")
+        mass_goal.addParameter(parameter, "p")
+        mass_goal.addParameter(parameter2, "q")
+
+        ms = study.initTropterSolver()
+        ms.set_num_mesh_intervals(25)
+        sol = study.solve()
+
+        self.assertAlmostEqual(sol.getParameter("sphere_mass") + sol.getParameter("sphere2_mass"),
+                               total_weight)