Undo binding of model getCoordinatesInMultibodyTreeOrder and introduc…

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")

Bindings/Python/tests/test_simbody.py

@@ -205,6 +205,9 @@ def test_SimbodyMatterSubsystem(self):
         assert (smss.calcSystemMassCenterLocationInGround(s)[2] == 
                 model.calcMassCenterPosition(s)[2])
 
+        coordNames = model.getCoordinateNamesInMultibodyTreeOrder();
+        print('firstCoord', coordNames.getElt(0));
+
         J = osim.Matrix()
         smss.calcSystemJacobian(s, J)
         # 6 * number of mobilized bodies
@@ -248,3 +251,5 @@ def test_SimbodyMatterSubsystem(self):
         assert residual.size() == s.getNU()
         for i in range(residual.size()):
             assert abs(residual[i]) < 1e-10
+
+

Bindings/simulation.i

@@ -177,9 +177,6 @@ OpenSim::ModelComponentSet<OpenSim::Controller>;
 %include <OpenSim/Simulation/Model/ModelVisualPreferences.h>
 %include <OpenSim/Simulation/Model/ModelVisualizer.h>
 %copyctor OpenSim::Model;
-%include <SimTKcommon/internal/ReferencePtr.h>
-%template(ReferencePtrCoordinate) SimTK::ReferencePtr<const OpenSim::Coordinate>;
-%template(StdVectorReferencePtrCoordinate) std::vector<SimTK::ReferencePtr<const OpenSim::Coordinate>>;
 %include <OpenSim/Simulation/Model/Model.h>
 
 %include <OpenSim/Simulation/Model/AbstractPathPoint.h>

CHANGELOG.md

@@ -25,7 +25,7 @@ and `Blankevoort1991Ligament`, usages of `get_GeometryPath`, `upd_GeometryPath`,
 - Deleting elements from an `OpenSim::Coordinate` range now throws an exception during `finalizeFromProperties` (previously:
   it would let you do it, and throw later when `Coordinate::getMinRange()` or `Coordinate::getMaxRange()` were called, #3532)
 - Added `FunctionBasedPath`, a class for representing paths in `Force`s based on `Function` objects (#3389)
-- Fixed bindings to expose the method Model::getCoordinatesInMultibodyTreeOrder to scripting users (#3569)
+- Introduced the method `Model::getCoordinateNamesInMultibodyTreeOrder` for convenient access to internal coordinate ordering for scripting users (#3569)
 - Fixed a bug where constructing a `ModelProcessor` from a `Model` object led to an invalid `Model`
 - Added `LatinHypercubeDesign`, a class for generating Latin hypercube designs using random and algorithm methods (#3570)
 - Refactor c3dExport.m file as a Matlab function (#3501), also expose method to allow some operations on tableColumns
@@ -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

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

OpenSim/Simulation/Model/Model.h

@@ -943,6 +943,14 @@ OpenSim_OBJECT_NONTEMPLATE_DEFS(Model, ModelComponent);
     std::vector<SimTK::ReferencePtr<const OpenSim::Coordinate>>
         getCoordinatesInMultibodyTreeOrder() const;
 
+    /** A variant of getCoordinatesInMultibodyTreeOrder that returns names for Scripting users */
+    SimTK::Array_<std::string> getCoordinateNamesInMultibodyTreeOrder() {
+        SimTK::Array_<std::string> namesArray;
+        auto coords = getCoordinatesInMultibodyTreeOrder();
+        for (int i=0; i < (int)coords.size(); ++i)
+            namesArray.push_back(coords[i]->getName());
+        return namesArray;
+    }
     /** Get a warning message if any Coordinates have a MotionType that is NOT
         consistent with its previous user-specified value that existed in 
         Model files prior to OpenSim 4.0 */

OpenSim/Simulation/Test/testAssemblySolver.cpp

@@ -131,7 +131,10 @@ void testAssembleModelWithConstraints(string modelFile)
     for(int i=0; i< coords.getSize(); i++) {
         cout << "Coordinate " << coords[i].getName() << " get value = " << coords[i].getValue(state) << endl;
     }
+    auto coordsInOrder = model.getCoordinateNamesInMultibodyTreeOrder();
+    cout << coordsInOrder << std::endl;
 
+    assert(coords.getSize()==coordsInOrder.size());
     // Initial coordinates after initial assembly
     Vector q0 = state.getQ();