Fix types for methods in BufferedOrientationReference (#3644) (#3669)

* Add missing types * Add tests to check BufferedOrientationReference * Update CHANGELOG * Add test case for appending data to oRefs Co-authored-by: Zach Strout <[email protected]>
opensim-org · Jan 10, 2024 · 34fd6af · 34fd6af
1 parent 0a67724
commit 34fd6af
Show file tree

Hide file tree

Showing 3 changed files with 25 additions and 4 deletions.
diff --git a/Bindings/Python/tests/test_opensense.py b/Bindings/Python/tests/test_opensense.py
@@ -33,6 +33,10 @@ def test_createObjects(self):
         constraint_var = .0001
         ikSolver = osim.InverseKinematicsSolver(model, mRefs, oRefs, coordinateReferences, constraint_var)
         print("Created InverseKinematicsSolver object..")
+        oRefs = osim.BufferedOrientationsReference()
+        print("Created BufferedOrientationsReference object..")
+        ikSolver = osim.InverseKinematicsSolver(model, mRefs, oRefs, coordinateReferences, constraint_var)
+        print("Created InverseKinematicsSolver object with BufferedOrientationsReference..")
 
     def test_vector_rowvector(self):
         print()
@@ -43,3 +47,19 @@ def test_vector_rowvector(self):
                 col[1] == row[1] and
                 col[2] == row[2] and
                 col[3] == row[3])
+
+    def test_BufferedOrientationReferencePut(self):
+        # Make sure that we can append new data to the BufferedOrientationReference object
+        quatTable = osim.TimeSeriesTableQuaternion(os.path.join(test_dir,'orientation_quats.sto'))
+        print("Created TimeSeriesTableQuaternion object..")
+        orientationsData = osim.OpenSenseUtilities.convertQuaternionsToRotations(quatTable)
+        print("Convert Quaternions to orientationsData")
+        time = 0
+        rowVecView = orientationsData.getNearestRow(time)
+        print("Sliced orientationData")
+        rowVec = osim.RowVectorRotation(rowVecView)
+        print("Converted slice to row vector")
+        oRefs = osim.BufferedOrientationsReference()
+        print("Created BufferedOrienationReference object")
+        oRefs.putValues(time, rowVec)
+        print("Added row vector to BufferedOrientationReference object")
diff --git a/CHANGELOG.md b/CHANGELOG.md
@@ -41,6 +41,7 @@ and `Blankevoort1991Ligament`, usages of `get_GeometryPath`, `upd_GeometryPath`,
 - Exposed simbody methods to obtain GravityForces, MobilityForces and BodyForces (#3490)
 - Simbody was updated such that the headers it transitively exposes to downstream projects are compatible with C++20 (#3619)
 - Moved speed computation from `computeForce` in children of `ScalarActuator` to dedicated `getSpeed` function.
+- Fix type problem with BufferedOrientationReference (#3415)
 
 
 v4.4.1

diff --git a/OpenSim/Simulation/BufferedOrientationsReference.h b/OpenSim/Simulation/BufferedOrientationsReference.h
@@ -33,7 +33,7 @@ namespace OpenSim {
 //=============================================================================
 /**
  * Subclass of OrientationsReference that handles live data by providing a DataQueue
- * that allows clients to push data into and allows the InverseKinematicsSolver to 
+ * that allows clients to push data into and allows the InverseKinematicsSolver to
  * draw data from for solving.
  * Ideally this would be templatized, allowing for all Reference classes to leverage it.
  *
@@ -79,19 +79,19 @@ class OSIMSIMULATION_API BufferedOrientationsReference
             SimTK::Array_<SimTK::Rotation_<double>>& values) const override;
 
     /** add passed in values to data procesing Queue */
-    void putValues(double time, const SimTK::RowVector_<SimTK::Rotation>& dataRow);
+    void putValues(double time, const SimTK::RowVector_<SimTK::Rotation_<double>>& dataRow);
 
     double getNextValuesAndTime(
             SimTK::Array_<SimTK::Rotation_<double>>& values) override;
 
     virtual bool hasNext() const override { return !_finished; };
 
-    void setFinished(bool finished) { 
+    void setFinished(bool finished) {
         _finished = finished;
     };
 private:
     // Use a specialized data structure for holding the orientation data
-    mutable DataQueue_<SimTK::Rotation> _orientationDataQueue;
+    mutable DataQueue_<SimTK::Rotation_<double>> _orientationDataQueue;
     bool _finished{false};
     //=============================================================================
 };  // END of class BufferedOrientationsReference