SynergyController

Moco/Moco/CMakeLists.txt

@@ -117,7 +117,7 @@ set(MOCO_SOURCES
         MocoConstraintInfo.cpp
         MocoStudyFactory.h
         MocoStudyFactory.cpp
-        )
+        Components/SynergyController.cpp Components/SynergyController.h)
 if (MOCO_WITH_TROPTER)
     list(APPEND MOCO_SOURCES
             tropter/TropterProblem.h

Moco/Moco/Components/SynergyController.cpp

@@ -0,0 +1,93 @@
+/* -------------------------------------------------------------------------- *
+ * OpenSim Moco: SynergyController.cpp                                        *
+ * -------------------------------------------------------------------------- *
+ * Copyright (c) 2020 Stanford University and the Authors                     *
+ *                                                                            *
+ * Author(s): Chris Dembia                                                    *
+ *                                                                            *
+ * 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 "SynergyController.h"
+
+#include "../MocoUtilities.h"
+
+using namespace OpenSim;
+
+SynergyController::SynergyController() {
+    constructProperties();
+}
+
+void SynergyController::constructProperties() {
+    constructProperty_synergy_weights();
+}
+
+void SynergyController::setSynergyWeights(
+        std::string actuatorControlName, SimTK::Vector weights) {
+    append_synergy_weights(SynergyWeightsVector(
+            std::move(actuatorControlName), std::move(weights)));
+}
+
+void SynergyController::extendFinalizeFromProperties() {
+    // TODO support non-scalar actuators.
+    int numActuatorControls = getActuatorSet().getSize();
+    OPENSIM_THROW_IF_FRMOBJ(
+            numActuatorControls != getProperty_synergy_weights().size(),
+            Exception,
+            fmt::format("Number of Actuators in this Controller ({}) must "
+                   "match the number of synergy weights vectors ({}).",
+                    numActuatorControls,
+                    getProperty_synergy_weights().size()));
+
+    if (numActuatorControls == 0) return;
+
+    int numSynergies = get_synergy_weights(0).get_weights().size();
+    m_synergyWeights.resize(numActuatorControls, numSynergies);
+
+    std::unordered_map<std::string, int> map;
+    for (int isw = 0; isw < getProperty_synergy_weights().size(); ++isw) {
+        const auto& synWeights = get_synergy_weights(isw);
+        map[synWeights.getName()] = isw;
+    }
+    for (int iact = 0; iact < numActuatorControls; ++iact) {
+        const auto& actuator = getActuatorSet().get(iact);
+        const auto actPath = actuator.getAbsolutePathString();
+
+        OPENSIM_THROW_IF_FRMOBJ(!map.count(actPath), Exception,
+                fmt::format("No synergy weights provided for Actuator '{}'.",
+                        actPath));
+        const auto& weights = get_synergy_weights(map[actPath]).get_weights();
+        OPENSIM_THROW_IF_FRMOBJ(weights.size() != numSynergies, Exception,
+                fmt::format("Length of synergy weights vectors is "
+                            "inconsistent ({} vs {}).",
+                        numSynergies, weights.size()));
+        m_synergyWeights.updRow(iact) = weights.transpose();
+    }
+}
+
+void SynergyController::computeControls(
+        const SimTK::State& s, SimTK::Vector& controls) const {
+    if (getActuatorSet().getSize() == 0) return;
+
+    const auto synergyControls =
+            getInputValue<SimTK::Vector>(s, "synergy_controls");
+
+    SimTK::Vector actuatorControls = m_synergyWeights * synergyControls;
+
+    SimTK::Vector thisControl(1);
+    for (int iact = 0; iact < getActuatorSet().getSize(); ++iact) {
+        const auto& actuator = getActuatorSet().get(iact);
+        thisControl[0] = actuatorControls[iact];
+        actuator.addInControls(thisControl,
+                // actuatorControls.block()(TODO, 0, actuator.numControls(), 1),
+                controls);
+    }
+}

Moco/Moco/Components/SynergyController.h

@@ -0,0 +1,90 @@
+#ifndef MOCO_SYNERGYCONTROLLER_H
+#define MOCO_SYNERGYCONTROLLER_H
+/* -------------------------------------------------------------------------- *
+ * OpenSim Moco: SynergyController.h                                          *
+ * -------------------------------------------------------------------------- *
+ * Copyright (c) 2020 Stanford University and the Authors                     *
+ *                                                                            *
+ * Author(s): Chris Dembia                                                    *
+ *                                                                            *
+ * 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 <OpenSim/Simulation/Model/Actuator.h>
+#include <OpenSim/Simulation/Control/Controller.h>
+#include "../osimMocoDLL.h"
+
+namespace OpenSim {
+
+// TODO how to handle stage?
+template <typename T>
+class OSIMMOCO_API Concatenator_ : public Component {
+    OpenSim_DECLARE_CONCRETE_OBJECT_T(Concatenator_, T, Component);
+public:
+    OpenSim_DECLARE_LIST_INPUT(inputs, T, SimTK::Stage::Dynamics, "TODO");
+    OpenSim_DECLARE_OUTPUT(output, SimTK::Vector_<T>, getConcatenatedVector,
+            SimTK::Stage::Dynamics);
+
+    SimTK::Vector_<T> getConcatenatedVector(const SimTK::State& s) const {
+        const auto& input = this->template getInput<T>("inputs");
+        const int numChannels = input.getNumConnectees();
+        SimTK::Vector_<T> output(numChannels);
+        for (int ichan = 0; ichan < numChannels; ++ichan) {
+            output[ichan] = input.getChannel(ichan).getValue(s);
+        }
+        return output;
+    }
+};
+
+using Concatenator = Concatenator_<double>;
+
+class OSIMMOCO_API SynergyWeightsVector : public Object {
+    OpenSim_DECLARE_CONCRETE_OBJECT(SynergyWeightsVector, Object);
+public:
+    OpenSim_DECLARE_PROPERTY(weights, SimTK::Vector, "TODO");
+    SynergyWeightsVector() {
+        constructProperty_weights(SimTK::Vector());
+    }
+    SynergyWeightsVector(std::string actuatorPath,
+            SimTK::Vector v) : SynergyWeightsVector() {
+        setName(std::move(actuatorPath));
+        set_weights(std::move(v));
+    }
+};
+
+class OSIMMOCO_API SynergyController : public Controller {
+    OpenSim_DECLARE_CONCRETE_OBJECT(SynergyController, Controller);
+public:
+    OpenSim_DECLARE_LIST_PROPERTY(synergy_weights, SynergyWeightsVector,
+            "TODO");
+    // TODO LIST_INPUT?
+    OpenSim_DECLARE_INPUT(synergy_controls, SimTK::Vector,
+            SimTK::Stage::Dynamics,
+            "The synergy control signals ordered TODO.");
+
+    SynergyController();
+
+    void setSynergyWeights(std::string actuator, SimTK::Vector weights);
+
+    void computeControls(
+            const SimTK::State& s, SimTK::Vector& controls) const override;
+protected:
+    void extendFinalizeFromProperties() override;
+
+private:
+    void constructProperties();
+
+    SimTK::Matrix m_synergyWeights;
+};
+
+} // namespace OpenSim
+
+#endif // MOCO_SYNERGYCONTROLLER_H

Moco/Moco/MocoProblem.h

@@ -362,7 +362,7 @@ class OSIMMOCO_API MocoPhase : public Object {
     OpenSim_DECLARE_PROPERTY(default_speed_bounds, MocoBounds,
             "Bounds for coordinate speeds if not specified in "
             "state_infos (default: [-50, 50]).");
-    OpenSim_DECLARE_PROPERTY(bound_activation_from_excitation, bool,	    
+    OpenSim_DECLARE_PROPERTY(bound_activation_from_excitation, bool,
             "For muscles without explicit activation bounds, set the bounds "
             "for muscle activation (if activation dynamics are enabled) from " 
             "the bounds for muscle control (excitation), using "