[WIP] Support for PrescribedController

.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/

Applications/Analyze/test/testAnalyzeTool.cpp

@@ -489,19 +489,12 @@ void testIMUDataReporter() {
                 pendulum.getComponent<CoordinateActuator>("/tau0"));
         controller->addActuator(
                 pendulum.getComponent<CoordinateActuator>("/tau1"));
-        pendulum.addController(controller);
-        pendulum.initSystem();
-
-        // Specify constant torque functions to the torque acutators via the
-        // PrescribedController we added previously.
+        // Specify constant torque functions to the torque actuators
         Constant* constantTorque0 = new Constant(10.0);
         Constant* constantTorque1 = new Constant(10.0);
-        pendulum.updComponent<PrescribedController>(
-                    "/controllerset/torque_controller")
-                .prescribeControlForActuator("tau0", constantTorque0);
-        pendulum.updComponent<PrescribedController>(
-                    "/controllerset/torque_controller")
-                .prescribeControlForActuator("tau1", constantTorque1);
+        controller->prescribeControlForActuator("tau0", constantTorque0);
+        controller->prescribeControlForActuator("tau1", constantTorque1);
+        pendulum.addController(controller);
         state = pendulum.initSystem();
 
         // Set a horizontal default pendulum position.

Applications/Forward/test/testForward.cpp

@@ -35,7 +35,7 @@ using namespace std;
 
 void testPendulum();
 void testPendulumExternalLoad();
-void testPendulumExternalLoadWithPointInGround(); 
+void testPendulumExternalLoadWithPointInGround();
 void testArm26();
 void testGait2354();
 void testGait2354WithController();
@@ -95,7 +95,7 @@ void testPendulumExternalLoad() {
     ASSERT(results.getLastTime() == 1.0);
 
     Storage standard("Results/pendulum_states.sto");
- 
+
 
     Array<double> data;
     int i = results.getSize() - 1;
@@ -104,10 +104,10 @@ void testPendulumExternalLoad() {
     data.setSize(state->getSize());
     standard.getDataAtTime(time, state->getSize(), data);
     int nc = forward.getModel().getNumCoordinates();
-    for (int j = 0; j < nc; ++j) {      
+    for (int j = 0; j < nc; ++j) {
         stringstream message;
-        message << "t=" << time <<" state# "<< j << " " 
-            << standard.getColumnLabels()[j+1] << " std=" << data[j] 
+        message << "t=" << time <<" state# "<< j << " "
+            << standard.getColumnLabels()[j+1] << " std=" << data[j]
             <<"  computed=" << state->getData()[j];
         ASSERT_EQUAL(data[j], state->getData()[j], 1e-2,
             __FILE__, __LINE__, "ASSERT_EQUAL FAILED " + message.str());
@@ -132,7 +132,7 @@ void testPendulumExternalLoadWithPointInGround() {
     data.setSize(state->getSize());
     standard.getDataAtTime(time, state->getSize(), data);
     int nc = forward.getModel().getNumCoordinates();
-    for (int j = 0; j < nc; ++j) {      
+    for (int j = 0; j < nc; ++j) {
         stringstream message;
         message << "t=" << time <<" state# "<< j << " " << standard.getColumnLabels()[j+1]
             << " std=" << data[j] <<"  computed=" << state->getData()[j];
@@ -160,7 +160,7 @@ void testArm26() {
     for (int j = 0; j < state->getSize(); ++j) {
         stringstream message;
         message << "t=" << time <<" state# "<< j << " " << standard->getColumnLabels()[j+1] << " std=" << data[j] <<"  computed=" << state->getData()[j] << endl;
-        ASSERT_EQUAL(data[j], state->getData()[j], 5.0e-3, 
+        ASSERT_EQUAL(data[j], state->getData()[j], 5.0e-3,
             __FILE__, __LINE__, "ASSERT_EQUAL FAILED " + message.str());
         cout << "ASSERT_EQUAL PASSED " << message.str();
     }
@@ -173,7 +173,7 @@ void testArm26() {
     for (int j = 0; j < state->getSize(); ++j) {
         stringstream message;
         message << "t=" << time <<" state# "<< j << " " << standard->getColumnLabels()[j+1] << " std=" << data[j] <<"  computed=" << state->getData()[j] << endl;
-        ASSERT_EQUAL(data[j], state->getData()[j], 5.0e-3, 
+        ASSERT_EQUAL(data[j], state->getData()[j], 5.0e-3,
             __FILE__, __LINE__, "ASSERT_EQUAL FAILED " + message.str());
         cout << "ASSERT_EQUAL PASSED " << message.str();
     }
@@ -189,7 +189,7 @@ void testGait2354()
     Storage* standard = new Storage();
     string statesFileName("std_subject01_walk1_states.sto");
     forward.loadStatesStorage( statesFileName, standard );
-    
+
     int nstates = forward.getModel().getNumStateVariables();
     int nq = forward.getModel().getNumCoordinates();
     std::vector<double> rms_tols(2*nstates, 0.001); //activations and fiber-lengths
@@ -198,11 +198,12 @@ void testGait2354()
         rms_tols[2*i] = 0.035; // coordinates at less than 2degrees
         rms_tols[2*i+1] = 2.5; // speeds can deviate by a lot due to open-loop test
     }
-    
-    CHECK_STORAGE_AGAINST_STANDARD(results, *standard, rms_tols, 
+
+    CHECK_STORAGE_AGAINST_STANDARD(results, *standard, rms_tols,
         __FILE__, __LINE__, "testGait2354 failed");
 }
 
+
 void testGait2354WithController() {
     ForwardTool forward("subject01_Setup_Forward_Controller.xml");
     forward.run();
@@ -220,7 +221,7 @@ void testGait2354WithController() {
         rms_tols[2*i] = 0.01; // coordinates at less than 0.6 degree
         rms_tols[2*i+1] = 0.1; // speeds should deviate less with feedback controller
     }
-    
+
     CHECK_STORAGE_AGAINST_STANDARD(results, *standard, rms_tols,
         __FILE__, __LINE__, "testGait2354WithController failed");
 }
@@ -242,8 +243,6 @@ void testGait2354WithControllerGUI() {
     forward.updateModelForces(*model, "");
     forward.setModel(*model);
 
-    model->initSystem();
-
     forward.run();
 
     // For good measure we'll make sure we still get the identical results

Bindings/Java/tests/TestModelBuilding.java

@@ -32,14 +32,13 @@ public static void main(String[] args) {
         PrescribedController cns = new PrescribedController();
         cns.addActuator(biceps);
         StepFunction testFunction = new StepFunction(0.5, 3.0, 0.3, 1.0);
-        cns.prescribeControlForActuator(0,      //Index in controller set
-            testFunction); 
+        cns.prescribeControlForActuator("biceps", testFunction);
         System.gc(); // Request gc could free testFunction and crash
         arm.addBody(hum);
         arm.addBody(rad);
         arm.addJoint(shoulder);
         arm.addJoint(elbow);
-        
+
         arm.addForce(biceps);
         arm.addController(cns);
         /*

Bindings/Python/tests/test_swig_additional_interface.py

@@ -291,11 +291,16 @@ def test_PrescribedController_prescribeControlForActuator(self):
         # Controller.
         contr = osim.PrescribedController()
         contr.addActuator(actu)
-        self.assertRaises(RuntimeError,
-                contr.prescribeControlForActuator, 1, osim.Constant(3))
-        # The following calls should not cause a memory leak:
-        contr.prescribeControlForActuator(0, osim.Constant(2))
         contr.prescribeControlForActuator('actu', osim.Constant(4))
+        model.addController(contr)
+        # Should not throw.
+        model.initSystem()
+
+        contr2 = osim.PrescribedController()
+        contr2.addActuator(actu)
+        contr2.prescribeControlForActuator('notAnActu', osim.Constant(5))
+        model.addController(contr2)
+        self.assertRaises(RuntimeError, model.initSystem)
 
     def test_set_iterator(self):
         fs = osim.FunctionSet()

CHANGELOG.md

@@ -9,8 +9,8 @@ This is not a comprehensive list of changes but rather a hand-curated collection
 v4.6
 ====
 - Added support for list `Socket`s via the macro `OpenSim_DECLARE_LIST_SOCKET`. The macro-generated method 
-`appendSocketConnectee_*` can be used to connect `Object`s to a list `Socket`. In addiion, `Component` and Socket have 
-new `getConnectee` overloads that take an index to a desired object in the list `Socket` (#3652).
+  `appendSocketConnectee_*` can be used to connect `Object`s to a list `Socket`. In addition, `Component` and Socket have 
+  new `getConnectee` overloads that take an index to a desired object in the list `Socket` (#3652).
 - Added `ComponentPath::root()`, which returns a `ComponentPath` equivalent to "/"
 - `ComponentPath` is now less-than (`<`) comparable, making it usable in (e.g.) `std::map`
 - `ComponentPath` now has a `std::hash<T>` implementation, making it usable in (e.g.) `std::unordered_map`
@@ -24,6 +24,12 @@ new `getConnectee` overloads that take an index to a desired object in the list
 - Calling `getConnectee` no longer strongly requires that `finalizeConnection` has been called on the socket. The
   implementation will now fall back to the (slower) method of following the socket's connectee path property. This
   is useful if (e.g.) following sockets *during* a call to `Component::finalizeConnections`
+- `Controller` now manages the list of controlled actuators using a list `Socket` instead of a `Set<Actuators>` (#3683).
+  The `actuator_list` property has been removed from `Controller` in lieu of the list `Socket`, which appears as 
+  `socket_actuators` in the XML. This change also necessitated the addition of an added `initSystem()` call in
+  `AbstractTool::updateModelForces()` so that connected actuators have the same root component as the `Model`
+  at the time of `Socket` connection. Finally, `PrescribedController::prescribeControlForActuator(int, Function*)` is
+  now deprecated in favor of `PrescribedController::prescribeControlForActuator(const std::string&, Function*)`.
 
 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/Common/ComponentSocket.h

@@ -180,7 +180,7 @@ class OSIMCOMMON_API AbstractSocket {
      * property to satisfy the socket. */
     virtual void disconnect() = 0;
 
-    /** %Set connectee path. This function can only be used if this socket is
+    /** Set the connectee path. This function can only be used if this socket is
      * not a list socket. If a connectee reference is set (with connect()) the
      * connectee path is ignored; call disconnect() if you want the socket to be
      * connected using the connectee path.
@@ -426,7 +426,7 @@ class Socket : public AbstractSocket {
      * connect to that component.
      *
      * Throws an exception If you provide only a component name and the
-     * model has multiple components with that nume.
+     * model has multiple components with that name.
      * */
     void findAndConnect(const ComponentPath& connectee) override;
 

OpenSim/Common/XMLDocument.cpp

@@ -67,9 +67,10 @@ using namespace std;
 // 30516 for GeometryPath default_color -> Appearance
 // 30517 for removal of _connectee_name suffix to shorten XML for socket, input
 // 40000 for OpenSim 4.0 release 40000
-// 40500 for updating 'GeometryPath' nodes to have property name 'path'.
+// 40500 for updating GeometryPath nodes to have property name 'path'.
+// 40600 for converting Controller actuators to a list Socket.
 
-const int XMLDocument::LatestVersion = 40500;
+const int XMLDocument::LatestVersion = 40600;
 //=============================================================================
 // DESTRUCTOR AND CONSTRUCTOR(S)
 //=============================================================================

OpenSim/ExampleComponents/ToyReflexController.cpp

@@ -66,20 +66,14 @@ void ToyReflexController::extendConnectToModel(Model &model)
 {
     Super::extendConnectToModel(model);
 
-    // get the list of actuators assigned to the reflex controller
-    Set<const Actuator>& actuators = updActuators();
-
-    int cnt=0;
-
-    while(cnt < actuators.getSize()){
-        const Muscle *musc = dynamic_cast<const Muscle*>(&actuators[cnt]);
-        // control muscles only
-        if(!musc){
-            log_warn("ToyReflexController assigned a non-muscle actuator '{}', "
-                     "which will be ignored.", actuators[cnt].getName());
-            actuators.remove(cnt);
-        }else
-            cnt++;
+    const auto& socket = getSocket<Actuator>("actuators");
+    for (int i = 0; i < (int)socket.getNumConnectees(); ++i) {
+        const auto& actu = socket.getConnectee(i);
+        const auto* musc = dynamic_cast<const Muscle*>(&actu);
+        OPENSIM_THROW_IF_FRMOBJ(!musc, Exception,
+            "Expected only muscle actuators assigned to this controller's "
+            "'actuators' socket, but the non-muscle actuator '{}' was found.",
+            actu.getName());
     }
 }
 
@@ -94,12 +88,10 @@ void ToyReflexController::extendConnectToModel(Model &model)
  * @param controls  system wide controls to which this controller can add
  */
 void ToyReflexController::computeControls(const State& s,
-                                          Vector &controls) const {   
-    // get time
-    s.getTime();
+                                          Vector &controls) const {
 
-    // get the list of actuators assigned to the reflex controller
-    const Set<const Actuator>& actuators = getActuatorSet();
+    // Get the Socket to the list of actuators assigned to the reflex controller.
+    const auto& socket = getSocket<Actuator>("actuators");
 
     // muscle lengthening speed
     double speed = 0;
@@ -108,8 +100,9 @@ void ToyReflexController::computeControls(const State& s,
     //reflex control
     double control = 0;
 
-    for(int i=0; i<actuators.getSize(); ++i){
-        const Muscle *musc = dynamic_cast<const Muscle*>(&actuators[i]);
+    for (int i = 0; i < (int)socket.getNumConnectees(); ++i) {
+        const auto& actu = socket.getConnectee(i);
+        const Muscle *musc = dynamic_cast<const Muscle*>(&actu);
         speed = musc->getLengtheningSpeed(s);
         // un-normalize muscle's maximum contraction velocity (fib_lengths/sec) 
         max_speed =

OpenSim/Examples/ControllerExample/ControllerExample.cpp

@@ -111,8 +111,9 @@ OpenSim_DECLARE_CONCRETE_OBJECT(TugOfWarController, Controller);
         double blockMass = getModel().getBodySet().get( "block" ).getMass();
 
         // Get pointers to each of the muscles in the model.
-        auto leftMuscle = dynamic_cast<const Muscle*>  ( &getActuatorSet().get(0) );
-        auto rightMuscle = dynamic_cast<const Muscle*> ( &getActuatorSet().get(1) );
+        const auto& socket = getSocket<Actuator>("actuators");
+        auto leftMuscle = dynamic_cast<const Muscle*>(&socket.getConnectee(0));
+        auto rightMuscle = dynamic_cast<const Muscle*>(&socket.getConnectee(1));
 
         // Compute the desired position of the block in the tug-of-war
         // model.

OpenSim/Examples/ExampleLuxoMuscle/LuxoMuscle_create_and_simulate.cpp

@@ -786,16 +786,20 @@ void createLuxoJr(OpenSim::Model& model){
     PrescribedController* kneeController = new PrescribedController();
     kneeController->addActuator(*kneeExtensorLeft);
     kneeController->addActuator(*kneeExtensorRight);
-    kneeController->prescribeControlForActuator(0, x_of_t);
-    kneeController->prescribeControlForActuator(1, x_of_t->clone());
+    kneeController->prescribeControlForActuator(
+        kneeExtensorLeft->getAbsolutePathString(), x_of_t);
+    kneeController->prescribeControlForActuator(
+        kneeExtensorRight->getAbsolutePathString(), x_of_t->clone());
 
     model.addController(kneeController);
 
     PrescribedController* backController = new PrescribedController();
     backController->addActuator(*backExtensorLeft);
     backController->addActuator(*backExtensorRight);
-    backController->prescribeControlForActuator(0, x_of_t->clone());
-    backController->prescribeControlForActuator(1, x_of_t->clone());
+    backController->prescribeControlForActuator(
+        backExtensorLeft->getAbsolutePathString(), x_of_t->clone());
+    backController->prescribeControlForActuator(
+        backExtensorRight->getAbsolutePathString(), x_of_t->clone());
 
     model.addController(backController);
 

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/ControlAllocator.h
+        Components/ControlAllocator.cpp
         MocoCasADiSolver/MocoCasADiSolver.h
         MocoCasADiSolver/MocoCasADiSolver.cpp
         MocoInverse.cpp