Merge pull request #3940 from opensim-org/analyze_rotation_bindings

Add bindings support, `analyze` utilities, and test coverage for `TimeSeriesTable_<SimTK::Rotation>`
opensim-org · Oct 17, 2024 · e8b4ace · e8b4ace
2 parents 37e40e1 + a11d3e5
commit e8b4ace
Show file tree

Hide file tree

Showing 9 changed files with 232 additions and 17 deletions.
diff --git a/Bindings/Java/tests/TestTables.java b/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() {

diff --git a/Bindings/Python/tests/test_DataTable.py b/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)

diff --git a/Bindings/common.i b/Bindings/common.i
@@ -253,6 +253,14 @@ DATATABLE_CLONE(double, SimTK::Rotation_<double>)
     packSpatialVec(std::vector<std::string> suffixes) {
         return $self->pack<SimTK::SpatialVec>();
     }
+    DataTable_<double, SimTK::Rotation_<double>>
+    packRotation() {
+        return $self->pack<SimTK::Rotation_<double>>();
+    }
+    DataTable_<double, SimTK::Rotation_<double>>
+    packRotation(std::vector<std::string> suffixes) {
+        return $self->pack<SimTK::Rotation_<double>>();
+    }
 }
 
 %ignore OpenSim::TimeSeriesTable_::TimeSeriesTable_(TimeSeriesTable_ &&);
@@ -294,6 +302,14 @@ DATATABLE_CLONE(double, SimTK::Rotation_<double>)
     packSpatialVec(std::vector<std::string> suffixes) {
         return $self->pack<SimTK::SpatialVec>();
     }
+    TimeSeriesTable_<SimTK::Rotation_<double>>
+    packRotation() {
+        return $self->pack<SimTK::Rotation_<double>>();
+    }
+    TimeSeriesTable_<SimTK::Rotation_<double>>
+    packRotation(std::vector<std::string> suffixes) {
+        return $self->pack<SimTK::Rotation_<double>>();
+    }
 }
 %extend OpenSim::TimeSeriesTable_<SimTK::Vec3> {
     TimeSeriesTable_<double> flatten() {
@@ -335,6 +351,14 @@ DATATABLE_CLONE(double, SimTK::Rotation_<double>)
         return $self->flatten(suffixes);
     }
 }
+%extend OpenSim::TimeSeriesTable_<SimTK::Rotation_<double>> {
+    TimeSeriesTable_<double> flatten() {
+        return $self->flatten();
+    }
+    TimeSeriesTable_<double> flatten(std::vector<std::string> suffixes) {
+        return $self->flatten(suffixes);
+    }
+}
 
 %include <OpenSim/Common/AbstractDataTable.h>
 %include <OpenSim/Common/DataTable.h>
@@ -371,7 +395,7 @@ DATATABLE_CLONE(double, SimTK::Rotation_<double>)
 %shared_ptr(OpenSim::IMUDataReader)
 %shared_ptr(OpenSim::XsensDataReader)
 %shared_ptr(OpenSim::APDMDataReader)
-%shared_ptr(OpenSim::STOFileAdapter_<duoble>)
+%shared_ptr(OpenSim::STOFileAdapter_<double>)
 %shared_ptr(OpenSim::STOFileAdapter_<SimTK::Vec3>)
 %shared_ptr(OpenSim::STOFileAdapter_<SimTK::UnitVec3>)
 %shared_ptr(OpenSim::STOFileAdapter_<SimTK::Quaternion>)

diff --git a/Bindings/moco.i b/Bindings/moco.i
@@ -138,6 +138,10 @@ namespace OpenSim {
 %include <OpenSim/Moco/MocoTropterSolver.h>
 %include <OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.h>
 %include <OpenSim/Moco/MocoStudy.h>
+%template(analyzeVec3) OpenSim::MocoStudy::analyze<SimTK::Vec3>;
+%template(analyzeSpatialVec) OpenSim::MocoStudy::analyze<SimTK::SpatialVec>;
+%template(analyzeRotation) OpenSim::MocoStudy::analyze<SimTK::Rotation_<double>>;
+
 %include <OpenSim/Moco/MocoStudyFactory.h>
 
 %include <OpenSim/Moco/MocoTool.h>
@@ -148,5 +152,6 @@ namespace OpenSim {
 %template(analyzeMocoTrajectory) OpenSim::analyzeMocoTrajectory<double>;
 %template(analyzeMocoTrajectoryVec3) OpenSim::analyzeMocoTrajectory<SimTK::Vec3>;
 %template(analyzeMocoTrajectorySpatialVec) OpenSim::analyzeMocoTrajectory<SimTK::SpatialVec>;
+%template(analyzeMocoTrajectoryRotation) OpenSim::analyzeMocoTrajectory<SimTK::Rotation_<double>>;
 
 %include <OpenSim/Moco/ModelOperatorsDGF.h>
diff --git a/Bindings/simulation.i b/Bindings/simulation.i
@@ -263,6 +263,7 @@ OpenSim::ModelComponentSet<OpenSim::Controller>;
 %template(analyze) OpenSim::analyze<double>;
 %template(analyzeVec3) OpenSim::analyze<SimTK::Vec3>;
 %template(analyzeSpatialVec) OpenSim::analyze<SimTK::SpatialVec>;
+%template(analyzeRotation) OpenSim::analyze<SimTK::Rotation_<double>>;
 
 %include <OpenSim/Simulation/VisualizerUtilities.h>
 

diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -93,6 +93,8 @@ pointer to avoid crashes in scripting due to invalid pointer ownership (#3781).
 - Fixed `MocoOrientationTrackingGoal::initializeOnModelImpl` to check for missing kinematic states, but allow other missing columns. (#3830)
 - Improved exception handling for internal errors in `MocoCasADiSolver`. Problems will now abort and print a descriptive error message (rather than fail due to an empty trajectory). (#3834)
 - Upgraded the Ipopt dependency Metis to version 5.1.0 on Unix and macOS to enable building on `osx-arm64` (#3874).
+- Added Python and Java (Matlab) scripting support for `TimeSeriesTable_<SimTK::Rotation>`. (#3940)
+- Added the templatized `MocoStudy::analyze<T>()` and equivalent scripting counterparts: `analyzeVec3`, `analyzeSpatialVec`, `analyzeRotation`. (#3940)
 
 v4.5
 ====

diff --git a/OpenSim/Common/Test/testDataTable.cpp b/OpenSim/Common/Test/testDataTable.cpp
@@ -243,6 +243,9 @@ TEST_CASE("DataTable") {
     ASSERT((static_cast<AbstractDataTable&&>
             (DataTable_<double, SpatialVec>{})).
             numComponentsPerElement() == 6);
+    ASSERT((static_cast<AbstractDataTable&&>
+            (DataTable_<double, Rotation>{})).
+            numComponentsPerElement() == 9);
 
     {
         std::cout << "Test DataTable flattening constructor for Vec3."
@@ -371,6 +374,26 @@ TEST_CASE("DataTable") {
         ASSERT(tableDouble.getNumColumns()          == 18);
 
         std::cout << tableDouble << std::endl;
+
+        std::cout << "Test DataTable flattening constructor for Rotation."
+                  << std::endl;
+        DataTable_<double, Rotation> tableRotation{};
+        tableRotation.setColumnLabels({"col0", "col1"});
+        tableRotation.appendRow(0.1, {Rotation(0.1, UnitVec3(1, 0, 0)),
+                                      Rotation(0.2, UnitVec3(0, 1, 0))});
+        tableRotation.appendRow(0.2, {Rotation(0.2, UnitVec3(0, 0, 1)),
+                                      Rotation(0.1, UnitVec3(0, 1, 0))});
+        tableRotation.appendRow(0.3, {Rotation(0.3, UnitVec3(0, 1, 0)),
+                                      Rotation(0.2, UnitVec3(1, 0, 0))});
+
+        std::cout << tableRotation << std::endl;
+
+        tableDouble = tableRotation;
+        ASSERT(tableDouble.getColumnLabels().size() == 18);
+        ASSERT(tableDouble.getNumRows()             == 3);
+        ASSERT(tableDouble.getNumColumns()          == 18);
+
+        std::cout << tableDouble << std::endl;
     }
     {
         std::cout << "Test TimeSeriesTable flattening constructor for Vec3"
@@ -525,6 +548,43 @@ TEST_CASE("DataTable") {
         ASSERT(tableDouble.getNumColumns()          == 18);
 
         std::cout << tableDouble << std::endl;
+
+        std::cout << "Test TimeSeriesTable flattening constructor for "
+                     "Rotation" << std::endl;
+        DataTable_<double, Rotation> tableRotation{};
+        tableRotation.setColumnLabels({"col0", "col1"});
+        tableRotation.appendRow(0.1, {Rotation(0.1, UnitVec3(1, 0, 0)),
+                                      Rotation(0.2, UnitVec3(0, 1, 0))});
+        tableRotation.appendRow(0.2, {Rotation(0.2, UnitVec3(0, 0, 1)),
+                                      Rotation(0.1, UnitVec3(0, 1, 0))});
+        tableRotation.appendRow(0.3, {Rotation(0.3, UnitVec3(0, 1, 0)),
+                                      Rotation(0.2, UnitVec3(1, 0, 0))});
+
+        // TODO: RowVector_<Rotation> is not supported.
+        // const auto& avgRowRot = tableRotation.averageRow(0.1, 0.2);
+        // for(int i = 0; i < 3; ++i) {
+        //     OPENSIM_THROW_IF(std::abs(avgRowRot[0][0][i] - 2) > 1e-8/*eps*/,
+        //                      OpenSim::Exception,
+        //                      "Test failed: averageRow() failed.");
+        // }
+
+        // const auto& nearRowRot = tableRotation.getNearestRow(0.29);
+        // for(int i = 0; i < 3; ++i)
+        //     ASSERT(nearRowRot[0][0][i] == 2);
+
+        // tableRotation.updNearestRow(0.29) += Rotation(0.2, UnitVec3(0, 1, 0));
+        // tableRotation.updNearestRow(0.29) -= Rotation(0.2, UnitVec3(0, 1, 0));
+        // for(int i = 0; i < 3; ++i)
+        //     ASSERT(nearRowRot[0][0][i] == 2);
+
+        std::cout << tableRotation << std::endl;
+
+        tableDouble = tableRotation;
+        ASSERT(tableDouble.getColumnLabels().size() == 18);
+        ASSERT(tableDouble.getNumRows()             == 3);
+        ASSERT(tableDouble.getNumColumns()          == 18);
+
+        std::cout << tableDouble << std::endl;
     }
     {
         std::cout << "Test DataTable packing." << std::endl;
@@ -601,6 +661,32 @@ TEST_CASE("DataTable") {
         ASSERT(tableSVec.getTableMetaData<std::string>("string") == "string");
         ASSERT(tableSVec.getTableMetaData<int>("int")            == 10);
         std::cout << tableSVec << std::endl;
+
+        std::cout << "Test DataTable packing for Rotation" << std::endl;
+        DataTable_<double, double> table{};
+        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"});
+        table.appendRow(1, RowVector(18, 1));
+        table.appendRow(2, RowVector(18, 2));
+        table.appendRow(3, RowVector(18, 3));
+        table.addTableMetaData("string", std::string{"string"});
+        table.addTableMetaData("int", 10);
+        ASSERT(table.getColumnLabels().size() == 18);
+        ASSERT(table.getNumRows()             == 3);
+        ASSERT(table.getNumColumns()          == 18);
+
+        auto tableRot = table.pack<SimTK::Rotation>();
+        expLabels = {"col0", "col1"};
+        ASSERT(tableRot.getColumnLabels() == expLabels);
+        ASSERT(tableRot.getNumRows()      == 3);
+        ASSERT(tableRot.getNumColumns()   == 2);
+        ASSERT(tableRot.getTableMetaData<std::string>("string") == "string");
+        ASSERT(tableRot.getTableMetaData<int>("int")            == 10);
+        std::cout << tableRot << std::endl;
     }
     {
         std::cout << "Test TimeSeriesTable packing." << std::endl;
@@ -692,6 +778,33 @@ TEST_CASE("DataTable") {
         ASSERT(tableSVec.getTableMetaData<std::string>("string") == "string");
         ASSERT(tableSVec.getTableMetaData<int>("int")            == 10);
         std::cout << tableSVec << std::endl;
+
+        std::cout << "Test TimeSeriesTable packing for Rotation" << std::endl;
+        TimeSeriesTable_<double> table{};
+        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"});
+        table.appendRow(1, RowVector(18, 1));
+        table.appendRow(2, RowVector(18, 2));
+        table.appendRow(3, RowVector(18, 3));
+        table.addTableMetaData("string", std::string{"string"});
+        table.addTableMetaData("int", 10);
+        ASSERT(table.getColumnLabels().size() == 18);
+        ASSERT(table.getNumRows()             == 3);
+        ASSERT(table.getNumColumns()          == 18);
+
+        TimeSeriesTable_<SimTK::Rotation> tableRot = 
+            table.pack<SimTK::Rotation>();
+        expLabels = {"col0", "col1"};
+        ASSERT(tableRot.getColumnLabels() == expLabels);
+        ASSERT(tableRot.getNumRows()      == 3);
+        ASSERT(tableRot.getNumColumns()   == 2);
+        ASSERT(tableRot.getTableMetaData<std::string>("string") == "string");
+        ASSERT(tableRot.getTableMetaData<int>("int")            == 10);
+        std::cout << tableRot << std::endl;
     }
 
     {

diff --git a/OpenSim/Moco/MocoStudy.cpp b/OpenSim/Moco/MocoStudy.cpp
@@ -108,12 +108,6 @@ void MocoStudy::visualize(const MocoTrajectory& trajectory) const {
     VisualizerUtilities::showMotion(model, trajectory.exportToStatesTable());
 }
 
-TimeSeriesTable MocoStudy::analyze(const MocoTrajectory& trajectory,
-        const std::vector<std::string>& outputPaths) const {
-    return OpenSim::analyzeMocoTrajectory<double>(
-            get_problem().createRep().getModelBase(), trajectory, outputPaths);
-}
-
 TimeSeriesTable MocoStudy::calcGeneralizedForces(
         const MocoTrajectory& trajectory,
         const std::vector<std::string>& forcePaths) const {