[Moco] Adding support for PrescribedController and future controller types

.gitignore

@@ -55,6 +55,7 @@ delete_this_to_stop_optimization*.txt
 .idea/editor.xml
 .idea/codeStyles/codeStyleConfig.xml
 .idea/codeStyles/Project.xml
+.idea/editor.xml
 
 # Sensitive or high-churn files:
 .idea/**/dataSources/

CHANGELOG.md

@@ -33,6 +33,8 @@ v4.6
 - Bumped the version of `ezc3d` to 1.5.8, which can now deal properly with Type-3 force platforms and c3d from Shadow
 - Added `StationDefinedFrame` component, which is a `Frame` component that automatically computes its position and
   orientation from `Station`s in the model
+- Models with `PrescribedController`s are now supported by Moco (#3701). Controls for actuators controlled by 
+  `PrescribedController`s are now excluded from the optimization problem.
 
 v4.5
 ====

OpenSim/Actuators/ModelFactory.cpp

@@ -135,6 +135,7 @@ Model ModelFactory::createSlidingPointMass() {
     Model model;
     model.setName("sliding_mass");
     auto* body = new Body("body", 1.0, SimTK::Vec3(0), SimTK::Inertia(0));
+    body->attachGeometry(new Sphere(0.05));
     model.addComponent(body);
 
     // Allows translation along x.

OpenSim/Actuators/ModelFactory.h

@@ -43,10 +43,11 @@ class OSIMACTUATORS_API ModelFactory {
     /// This is a convenience for `createNLinkPendulum(2)`.
     static Model createDoublePendulum() { return createNLinkPendulum(2); }
     /// This model contains:
-    /// - 1 body: mass 1.0 kg, `/bodyset/body`.
+    /// - 1 body: mass 1.0 kg, `/body`.
     /// - 1 joint: SliderJoint along x axis, `/jointset/slider`, with
-    ///            coordinate `/jointset/slider/position`.
-    /// - 1 actuator: CoordinateActuator, controls [-10, 10], `/actuator`.
+    ///            coordinate `/slider/position`.
+    /// - 1 actuator: CoordinateActuator, controls [-10, 10],
+    ///              `/forceset/actuator`.
     /// Gravity is default; that is, (0, -g, 0).
     static Model createSlidingPointMass();
     /// This model contains:

OpenSim/Moco/CMakeLists.txt

@@ -82,14 +82,14 @@ set(MOCO_SOURCES
         MocoFrameDistanceConstraint.cpp
         MocoOutputConstraint.h
         MocoOutputConstraint.cpp
-        Components/DiscreteController.cpp
-        Components/DiscreteController.h
         Components/StationPlaneContactForce.h
         Components/StationPlaneContactForce.cpp
         Components/DiscreteForces.h
         Components/DiscreteForces.cpp
         Components/AccelerationMotion.h
         Components/AccelerationMotion.cpp
+        Components/ControlDistributor.h
+        Components/ControlDistributor.cpp
         MocoCasADiSolver/MocoCasADiSolver.h
         MocoCasADiSolver/MocoCasADiSolver.cpp
         MocoInverse.cpp

OpenSim/Moco/Components/ControlDistributor.cpp

@@ -0,0 +1,106 @@
+/* -------------------------------------------------------------------------- *
+*                      OpenSim: ControlDistributor.cpp                        *
+ * -------------------------------------------------------------------------- *
+ * Copyright (c) 2024 Stanford University and the Authors                     *
+ *                                                                            *
+ * Author(s): Nicholas Bianco                                                 *
+ *                                                                            *
+ * 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 "ControlDistributor.h"
+
+using namespace OpenSim;
+
+//=============================================================================
+// CONSTRUCTOR(S) AND DESTRUCTOR
+//=============================================================================
+ControlDistributor::ControlDistributor() = default;
+
+ControlDistributor::~ControlDistributor() noexcept = default;
+
+ControlDistributor::ControlDistributor(const ControlDistributor&) = default;
+
+ControlDistributor::ControlDistributor(ControlDistributor&&) = default;
+
+ControlDistributor& ControlDistributor::operator=(
+        const ControlDistributor&) = default;
+
+ControlDistributor& ControlDistributor::operator=(
+        ControlDistributor&&) = default;
+
+//=============================================================================
+// GET AND SET
+//=============================================================================
+void ControlDistributor::addControl(const std::string& controlName) {
+    const int index = (int)m_controlIndexMap.size();
+    m_controlIndexMap[controlName] = index;
+}
+
+void ControlDistributor::setControls(SimTK::State& s,
+        const SimTK::Vector& controls) const {
+    updControls(s) = controls;
+}
+
+SimTK::Vector& ControlDistributor::updControls(SimTK::State& s) const {
+    const SimTK::Subsystem& subSys = getSystem().getDefaultSubsystem();
+    auto& dv = subSys.updDiscreteVariable(s, m_discreteVarIndex);
+    auto& discreteControls =
+            SimTK::Value<SimTK::Vector>::updDowncast(dv).upd();
+    return discreteControls;
+}
+
+const SimTK::Vector& ControlDistributor::getControls(
+        const SimTK::State& s) const {
+    const SimTK::Subsystem& subSys = getSystem().getDefaultSubsystem();
+    auto& dv = subSys.getDiscreteVariable(s, m_discreteVarIndex);
+    auto& discreteControls = SimTK::Value<SimTK::Vector>::downcast(dv).get();
+    return discreteControls;
+}
+
+double ControlDistributor::getControlForOutputChannel(const SimTK::State& s,
+        const std::string& channel) const {
+    return getControls(s)[m_controlIndexMap.at(channel)];
+}
+
+std::vector<std::string> ControlDistributor::getControlNamesInOrder() const {
+    // Return the control names in ascending order of their indices.
+    std::vector<std::string> names;
+    names.reserve(m_controlIndexMap.size());
+    for (const auto& kv : m_controlIndexMap) {
+        names.push_back(kv.first);
+    }
+    std::sort(names.begin(), names.end(),
+            [&](const std::string& a, const std::string& b) {
+                return m_controlIndexMap.at(a) < m_controlIndexMap.at(b);
+            });
+    return names;
+}
+
+//=============================================================================
+// MODEL COMPONENT INTERFACE
+//=============================================================================
+void ControlDistributor::extendRealizeTopology(SimTK::State& state) const {
+    Super::extendRealizeTopology(state);
+    const SimTK::Subsystem& subSys = getSystem().getDefaultSubsystem();
+    const SimTK::Vector initControls(
+            static_cast<int>(m_controlIndexMap.size()), 0.0);
+    m_discreteVarIndex =
+            subSys.allocateDiscreteVariable(state, SimTK::Stage::Dynamics,
+                    new SimTK::Value<SimTK::Vector>(initControls));
+}
+
+void ControlDistributor::extendFinalizeFromProperties() {
+    Super::extendFinalizeFromProperties();
+    for (const auto& kv : m_controlIndexMap) {
+        updOutput("controls").addChannel(kv.first);
+    }
+}

OpenSim/Moco/Components/ControlDistributor.h

@@ -0,0 +1,156 @@
+#ifndef OPENSIM_CONTROL_DISTRIBUTOR_H
+#define OPENSIM_CONTROL_DISTRIBUTOR_H
+/* -------------------------------------------------------------------------- *
+ *                      OpenSim: ControlDistributor.h                         *
+ * -------------------------------------------------------------------------- *
+ * Copyright (c) 2024 Stanford University and the Authors                     *
+ *                                                                            *
+ * Author(s): Nicholas Bianco                                                 *
+ *                                                                            *
+ * 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/ModelComponent.h>
+#include <OpenSim/Moco/osimMocoDLL.h>
+
+namespace OpenSim {
+
+/**
+ * This component stores a vector of control values that can be used to
+ * allocate controls to other components in a model.
+ *
+ * A control value can be added to the ControlDistributor using `addControl()`.
+ * This adds a channel to the Output `controls` which is then available to
+ * distribute to other components via `Input` connections.
+ *
+ * The control values are stored in a discrete variable in the state. The
+ * control values are stored in the same order they were added to the
+ * distributor. The control values can be set and retrieved using the
+ * `setControls()`, `updControls()`, and `getControls()` methods.
+ *
+ * This class is primarily intended for use in Moco where it is used to
+ * distribute control values from an optimal control problem to InputControllers
+ * in a model. Therefore, "controls" in the context of this class do not
+ * necessarily correspond to controls in a model (i.e., from
+ * Model::getControls()). However, this class is generally useful for
+ * distributing scalar values stored in a discrete variable to other components
+ * in a model that accept an `Input`.
+ *
+ * ## Usage
+ *
+ * Constructing a ControlDistributor and adding controls:
+ * @code
+ * auto controlDistributor = make_unique<ControlDistributor>();
+ * controlDistributor->addControl("/forceset/soleus_r");
+ * controlDistributor->addControl("/my_input_controller_value");
+ * controlDistributor->addControl("/my_custom_component_input");
+ * model.addComponent(controlDistributor.release());
+ * @endcode
+ *
+ * Connecting all `Output` controls to another component:
+ * @code
+ * const auto& output = controlDistributor->getOutput("controls");
+ * auto& controller =
+ *     model.updComponent<ActuatorInputController>("/my_actu_controller");
+ * controller.connectInput_inputs(output);
+ *@endcode
+ *
+ * Connecting an individual control channel using an alias:
+ * @code
+ * const auto& output = controlDistributor->getOutput("controls");
+ * const auto& channel = output.getChannel("/forceset/soleus_r");
+ * auto& controller =
+ *     model.updComponent<ActuatorInputController>("/my_actu_controller");
+ * controller.connectInput_inputs(channel, "/forceset/soleus_r");
+ * @endcode
+ */
+class OSIMMOCO_API ControlDistributor : public ModelComponent {
+    OpenSim_DECLARE_CONCRETE_OBJECT(ControlDistributor, ModelComponent);
+
+public:
+//=============================================================================
+// OUTPUTS
+//=============================================================================
+    OpenSim_DECLARE_LIST_OUTPUT(controls, double, getControlForOutputChannel,
+            SimTK::Stage::Dynamics);
+
+//=============================================================================
+// METHODS
+//=============================================================================
+
+    // CONSTRUCTION AND DESTRUCTION
+    ControlDistributor();
+    ~ControlDistributor() noexcept override;
+
+    ControlDistributor(const ControlDistributor&);
+    ControlDistributor(ControlDistributor&&);
+
+    ControlDistributor& operator=(const ControlDistributor&);
+    ControlDistributor& operator=(ControlDistributor&&);
+
+    // GET AND SET
+    /**
+     * Add a control to the control distributor.
+     */
+    void addControl(const std::string& controlName);
+
+    /**
+     * Set the distributor controls stored in the discrete variable.
+     *
+     * The `controls` vector must be the same size as the number of controls
+     * added to the distributor by `addControl()`.
+     */
+    void setControls(SimTK::State& s, const SimTK::Vector& controls) const;
+
+    /**
+     * Get a writable reference to the distributor controls stored in the
+     * discrete variable.
+     */
+    SimTK::Vector& updControls(SimTK::State& s) const;
+
+    /**
+     * Get a const reference to the distributor controls stored in the discrete
+     * variable.
+     */
+    const SimTK::Vector& getControls(const SimTK::State& s) const;
+
+    /**
+     * Get the control value for the requested output channel.
+     */
+    double getControlForOutputChannel(
+            const SimTK::State& s, const std::string& channel) const;
+
+    /**
+     * Get the names of the controls in the order they were added to the
+     * distributor.
+     */
+    std::vector<std::string> getControlNamesInOrder() const;
+
+    /**
+     * Get a map of control names to their indices in the control vector.
+     */
+    std::unordered_map<std::string, int> getControlIndexMap() const {
+        return m_controlIndexMap;
+    }
+
+protected:
+    // MODEL COMPONENT INTERFACE
+    void extendRealizeTopology(SimTK::State& state) const override;
+    void extendFinalizeFromProperties() override;
+
+private:
+    std::unordered_map<std::string, int> m_controlIndexMap;
+    mutable SimTK::DiscreteVariableIndex m_discreteVarIndex;
+};
+
+} // namespace OpenSim
+
+#endif // OPENSIM_CONTROL_DISTRIBUTOR_H