[WIP] Allow live ik

OpenSim/Common/DataQueue.h

@@ -0,0 +1,120 @@
+#ifndef OPENSIM_DATA_QUEUE_H_
+#define OPENSIM_DATA_QUEUE_H_
+/* -------------------------------------------------------------------------- *
+ *                           OpenSim:  DataQueue.h                            *
+ * -------------------------------------------------------------------------- *
+ * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
+ * See http://opensim.stanford.edu and the NOTICE file for more information.  *
+ * OpenSim is developed at Stanford University and supported by the US        *
+ * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
+ * through the Warrior Web program.                                           *
+ *                                                                            *
+ * Copyright (c) 2005-2020 Stanford University and the Authors                *
+ * Author(s): Ayman Habib                                                     *
+ *                                                                            *
+ * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
+ * not use this file except in compliance with the License. You may obtain a  *
+ * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
+ *                                                                            *
+ * Unless required by applicable law or agreed to in writing, software        *
+ * distributed under the License is distributed on an "AS IS" BASIS,          *
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
+ * See the License for the specific language governing permissions and        *
+ * limitations under the License.                                             *
+ * -------------------------------------------------------------------------- */
+#include <queue>
+#include <condition_variable>
+#include <SimTKcommon.h>
+#include <OpenSim/Common/osimCommonDLL.h>
+
+namespace OpenSim {
+
+//=============================================================================
+//=============================================================================
+/**
+ * This base class defines the interface for a DataQueue. A data structure
+ * to maintain a queue of data to be passed between computations that are potentially
+ * different in processing speeds, decoupling the producers (e.g. File or live stream)
+ * from consumers. Synchronization mechanism will be implmented to allow handling of 
+ * multiple threads or significant differences in speeds.
+ *
+ * @author Ayman Habib
+ */
+/** Template class to contain Queue Entries, typically timestamped */
+template <class U> 
+class DataQueueEntry_ {
+public:
+    DataQueueEntry_(double timeStamp, const SimTK::RowVectorView_<U>& data)
+            : _timeStamp(timeStamp), _data(data){};
+    DataQueueEntry_(const DataQueueEntry_& other): _data(other._data)
+    {
+        _timeStamp = other.getTimeStamp();
+    };
+    DataQueueEntry_(DataQueueEntry_&&) = default;
+
+    DataQueueEntry_& operator=(const DataQueueEntry_&) { return (*this); };
+    virtual ~DataQueueEntry_(){};
+
+    double getTimeStamp() const { return _timeStamp; };
+    SimTK::RowVectorView_<U> getData() const { return _data; };
+
+private:
+    double _timeStamp;
+    SimTK::RowVectorView_<U> _data;
+};
+/**
+ * DataQueue is a wrapper around the std::queue customized to handle data processing
+ * and synchronization, and limiting the interface to only the subset of operations 
+ * needed for this use case. Synchronization is experimental as of now.
+ */
+template<class T> class DataQueue_ {
+//=============================================================================
+// METHODS
+//=============================================================================
+public:
+    //--------------------------------------------------------------------------
+    // CONSTRUCTION
+    //--------------------------------------------------------------------------
+    virtual ~DataQueue_() {}
+
+    DataQueue_()                                = default;
+    DataQueue_(const DataQueue_&){};
+    DataQueue_(DataQueue_&&){};
+    DataQueue_& operator=(const DataQueue_&) { 
+        return (*this);
+    };
+
+    //--------------------------------------------------------------------------
+    // DataQueue Interface
+    //--------------------------------------------------------------------------
+    void push_back(const double time, const SimTK::RowVectorView_<T>& data) { 
+        std::unique_lock<std::mutex> mlock(m_mutex);
+        m_data_queue.push(DataQueueEntry_<T>(time, data));
+        mlock.unlock();     // unlock before notificiation to minimize mutex con
+        m_cond.notify_one(); 
+    }
+    void pop_front(double& time, SimTK::RowVector_<T>& data) { 
+        std::unique_lock<std::mutex> mlock(m_mutex);
+        while (empty()) { m_cond.wait(mlock); }
+        DataQueueEntry_<SimTK::Rotation> frontEntry = m_data_queue.front();
+        m_data_queue.pop();
+        mlock.unlock(); 
+        time = frontEntry.getTimeStamp();
+        data = frontEntry.getData();
+    }
+    bool empty() const { 
+        return m_data_queue.empty(); 
+    }
+
+private:
+    // As of now we use std::queue but other data structures could be used as well
+    std::queue<DataQueueEntry_<T>> m_data_queue;
+    std::mutex m_mutex;
+    std::condition_variable m_cond;
+
+    //=============================================================================
+};  // END of class templatized DataQueue_<T>
+//=============================================================================
+}
+
+#endif // OPENSIM_DATA_QUEUE_H_

OpenSim/Simulation/BufferedOrientationsReference.cpp

@@ -0,0 +1,75 @@
+/* -------------------------------------------------------------------------- *
+ *                   OpenSim:  BufferedOrientationsReference.cpp              *
+ * -------------------------------------------------------------------------- *
+ * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
+ * See http://opensim.stanford.edu and the NOTICE file for more information.  *
+ * OpenSim is developed at Stanford University and supported by the US        *
+ * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
+ * through the Warrior Web program.                                           *
+ *                                                                            *
+ * Copyright (c) 2005-2020 Stanford University and the Authors                *
+ * Author(s): Ayman Habib                                                     *
+ *                                                                            *
+ * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
+ * not use this file except in compliance with the License. You may obtain a  *
+ * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
+ *                                                                            *
+ * Unless required by applicable law or agreed to in writing, software        *
+ * distributed under the License is distributed on an "AS IS" BASIS,          *
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
+ * See the License for the specific language governing permissions and        *
+ * limitations under the License.                                             *
+ * -------------------------------------------------------------------------- */
+#include "BufferedOrientationsReference.h"
+#include "OrientationsReference.h"
+#include <OpenSim/Common/Units.h>
+#include <OpenSim/Common/TRCFileAdapter.h>
+#include <SimTKcommon/internal/State.h>
+
+using namespace std;
+using namespace SimTK;
+
+namespace OpenSim {
+
+BufferedOrientationsReference::BufferedOrientationsReference()
+        : OrientationsReference() {
+    setAuthors("Ayman Habib");
+}
+
+BufferedOrientationsReference::BufferedOrientationsReference(
+        const OrientationsReference& orientationsRef)
+    : OrientationsReference(orientationsRef) {
+    // populate Queue from Table, solver will draw from the queue
+    /*
+    const std::vector<double>& times = orientationsRef.getTimes();
+    for (int i = 0; i < times.size(); ++i) {
+        _orientationDataQueue.push_back(times[i], _orientationData.getRowAtIndex(i));
+    }*/
+}
+/** get the values of the OrientationsReference */
+void BufferedOrientationsReference::getValues(
+        const SimTK::State& s,
+    SimTK::Array_<Rotation> &values) const
+{
+    auto& times = _orientationData.getIndependentColumn();
+    double time = s.getTime();
+    SimTK::RowVector_<SimTK::Rotation> nextRow;
+
+    if (s.getTime() >= times.front() && s.getTime() <= times.back()) {
+        nextRow = _orientationData.getRow(time);
+    } else {
+        _orientationDataQueue.pop_front(time, nextRow);
+    }
+    int n = nextRow.size();
+    values.resize(n);
+
+    for (int i = 0; i < n; ++i) { 
+        values[i] = nextRow[i];
+    }
+}
+
+void BufferedOrientationsReference::putValues(
+        double time, const SimTK::RowVector_<SimTK::Rotation>& dataRow) {
+    _orientationDataQueue.push_back(time, dataRow);
+}
+} // end of namespace OpenSim

OpenSim/Simulation/BufferedOrientationsReference.h

@@ -0,0 +1,88 @@
+#ifndef OPENSIM_BUFFERED_ORIENTATIONS_REFERENCE_H_
+#define OPENSIM_BUFFERED_ORIENTATIONS_REFERENCE_H_
+/* -------------------------------------------------------------------------- *
+ *                     OpenSim:  BufferedOrientationsReference.h              *
+ * -------------------------------------------------------------------------- *
+ * The OpenSim API is a toolkit for musculoskeletal modeling and simulation.  *
+ * See http://opensim.stanford.edu and the NOTICE file for more information.  *
+ * OpenSim is developed at Stanford University and supported by the US        *
+ * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA    *
+ * through the Warrior Web program.                                           *
+ *                                                                            *
+ * Copyright (c) 2005-2020 Stanford University and the Authors                *
+ * Author(s): Ayman Habib                                                     *
+ *                                                                            *
+ * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
+ * not use this file except in compliance with the License. You may obtain a  *
+ * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
+ *                                                                            *
+ * Unless required by applicable law or agreed to in writing, software        *
+ * distributed under the License is distributed on an "AS IS" BASIS,          *
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
+ * See the License for the specific language governing permissions and        *
+ * limitations under the License.                                             *
+ * -------------------------------------------------------------------------- */
+
+#include "OrientationsReference.h"
+#include <OpenSim/Common/DataQueue.h>
+
+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 
+ * draw data from for solving.
+ * Ideally this would be templatized, allowing for all Reference classes to leverage it.
+ *
+ * @author Ayman Habib
+ */
+class OSIMSIMULATION_API BufferedOrientationsReference : public OrientationsReference {
+    OpenSim_DECLARE_CONCRETE_OBJECT(
+            BufferedOrientationsReference, OrientationsReference);
+ //=============================================================================
+// METHODS
+//=============================================================================
+public:
+    //--------------------------------------------------------------------------
+    // CONSTRUCTION
+    //--------------------------------------------------------------------------
+    BufferedOrientationsReference();
+    BufferedOrientationsReference(
+            const BufferedOrientationsReference&) = default;
+    BufferedOrientationsReference(BufferedOrientationsReference&&) = default;
+    BufferedOrientationsReference& operator=(
+            const BufferedOrientationsReference&) = default;
+    BufferedOrientationsReference(const OrientationsReference& orientationsRef);
+
+    virtual ~BufferedOrientationsReference() {}
+
+    //--------------------------------------------------------------------------
+    // Reference Interface
+    //--------------------------------------------------------------------------
+    /** get the time range for which the OrientationsReference values are valid,
+        based on the loaded orientation data.*/
+    SimTK::Vec2 getValidTimeRange() const override{
+        return SimTK::Vec2(-SimTK::Infinity, SimTK::Infinity);
+    };
+
+    /** get the values from either the base OrientationsReference, or from
+     * the client provided data that was queued earlier using putValues call. */
+    void getValues(const SimTK::State& s,
+            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);
+
+private:
+    // Use a specialized data structure for holding the orientation data
+    mutable DataQueue_<SimTK::Rotation> _orientationDataQueue;
+
+//=============================================================================
+};  // END of class OrientationsReference
+//=============================================================================
+} // namespace
+
+#endif // OPENSIM_BUFFERED_ORIENTATIONS_REFERENCE_H_

OpenSim/Simulation/InverseKinematicsSolver.cpp

@@ -51,12 +51,11 @@ InverseKinematicsSolver::InverseKinematicsSolver(const Model& model,
     const MarkersReference& markersReference,
     const OrientationsReference& orientationsReference,
     SimTK::Array_<CoordinateReference>& coordinateReferences,
-    double constraintWeight ) :
-        AssemblySolver(model, coordinateReferences, constraintWeight)
-{
+        double constraintWeight):
+          AssemblySolver(model, coordinateReferences, constraintWeight),
+          _orientationsReference(orientationsReference) {
     // InverseKinematicsSolver has its own internal copy of the References to track
     _markersReference = markersReference;
-    _orientationsReference = orientationsReference;
 
     setAuthors("Ajay Seth");
 

OpenSim/Simulation/InverseKinematicsSolver.h

@@ -25,7 +25,7 @@
 
 #include "AssemblySolver.h"
 #include "MarkersReference.h"
-#include "OrientationsReference.h"
+#include "BufferedOrientationsReference.h"
 
 namespace SimTK {
 class Markers;
@@ -204,6 +204,11 @@ class OSIMSIMULATION_API InverseKinematicsSolver: public AssemblySolver
     orientations returned by the solver. */
     std::string getOrientationSensorNameForIndex(int osensorIndex) const;
 
+    void addOrientationValuesToTrack(
+            double time, SimTK::RowVector_<SimTK::Rotation_<double>>& data){
+        _orientationsReference.putValues(time, data);
+    };
+
 protected:
     /** Override to include point of interest matching (Marker tracking)
         as well ad Frame orientation (OSensor) tracking.
@@ -225,7 +230,7 @@ class OSIMSIMULATION_API InverseKinematicsSolver: public AssemblySolver
     MarkersReference _markersReference;
 
     // The orientation reference values and weightings
-    OrientationsReference _orientationsReference;
+    BufferedOrientationsReference _orientationsReference;
 
     // Non-accessible cache of the marker values to be matched at a given state
     SimTK::Array_<SimTK::Vec3> _markerValues;

OpenSim/Simulation/OrientationsReference.h

@@ -146,9 +146,11 @@ class OSIMSIMULATION_API OrientationsReference : public Reference_<SimTK::Rotati
     void constructProperties();
     void populateFromOrientationData();
 
-private:
+protected:
     // Use a specialized data structure for holding the orientation data
     TimeSeriesTable_<SimTK::Rotation> _orientationData;
+
+private:
     // orientation names inside the orientation data
     SimTK::Array_<std::string> _orientationNames;
     // corresponding list of weights guaranteed to be in the same order as names above