Merge pull request #2166 from opensim-org/fix_correction_controller_with_GUI_use_case

Fix correction controller with GUI use case

Author: aseth1

Applications/Forward/test/subject01_Setup_Forward_Controller.xml

@@ -60,8 +60,8 @@
 					    indicates the controller will controll all the acuators in the model-->
 					<actuator_list> </actuator_list>
 					<!--Flag (true or false) indicating whether or not the controller is
-					    enabled (ON) should-->
-					<enable_controller> true </enable_controller>
+					    disabled.-->
+					<isDisabled> false </isDisabled>
 					<!--XML file containing the controls for the controlSet.-->
 					<controls_file> subject01_walk1_controls.xml </controls_file>
 				</ControlSetController>
@@ -70,12 +70,12 @@
 					    indicates the controller will controll all the acuators in the model-->
 					<actuator_list> </actuator_list>
 					<!--Flag (true or false) indicating whether or not the controller is
-					    enabled (ON) should-->
-					<enable_controller> true </enable_controller>
+					    disabled.-->
+					<isDisabled> false </isDisabled>
 					<!--Gain for position errors-->
-					<kp>     25.00000000 </kp>
+					<kp>    16.00000000 </kp>
 					<!--Gain for velocity errors-->
-					<kv>      10.00000000 </kv>
+					<kv>     8.00000000 </kv>
 				</CorrectionController>
 			</objects>
 			<groups/>

Applications/Forward/test/testForward.cpp

@@ -33,60 +33,34 @@
 using namespace OpenSim;
 using namespace std;
 
-void testPendulum();    // test manager/integration process
-void testPendulumExternalLoad(); // test application of external loads point in pendulum
-void testPendulumExternalLoadWithPointInGround(); // test application of external loads point in ground
-void testArm26();       // now add computation of controls and generation of muscle forces
-void testGait2354();    // controlled muscles and ground reactions forces 
-void testGait2354WithController(); // included additional controller
+void testPendulum();
+void testPendulumExternalLoad();
+void testPendulumExternalLoadWithPointInGround(); 
+void testArm26();
+void testGait2354();
+void testGait2354WithController();
+void testGait2354WithControllerGUI();
+
 
 int main() {
     Object::renameType("Thelen2003Muscle", "Thelen2003Muscle_Deprecated");
 
-    SimTK::Array_<std::string> failures;
-
-    // test manager/integration process
-    try { testPendulum(); cout << "\nPendulum test PASSED " << endl; }  
-    catch (const std::exception& e)
-        { cout << e.what() <<endl; failures.push_back("testPendulum"); }
-    
-    // test application of external loads
-    try { testPendulumExternalLoad(); 
-        cout << "\nPendulum with external load test PASSED " << endl; }
-    catch (const std::exception& e)
-        { cout << e.what() <<endl; failures.push_back("testPendulumExternalLoad"); }
-    
-    // test application of external loads
-    try { testPendulumExternalLoadWithPointInGround(); 
-        cout << "\nPendulum with external load and point in ground PASSED " << endl; }
-    catch (const std::exception& e)
-        { cout << e.what() <<endl; failures.push_back("testPendulumExternalLoadWithPointInGround"); }
-    
-    // now add computation of controls and generation of muscle forces
-    try { testArm26(); 
-        cout << "\narm26 test PASSED " << endl; }
-    catch (const std::exception& e)
-        { cout << e.what() <<endl; failures.push_back("testArm26"); }
-        
-    // include applied ground reactions forces 
-    try { testGait2354(); 
-        cout << "\ngait2354 test PASSED " << endl; }
-    catch (const std::exception& e)
-        { cout << e.what() <<endl; failures.push_back("testGait2354"); }
-
-    // finally include a controller
-    try { testGait2354WithController(); 
-        cout << "\ngait2354 with correction controller test PASSED " << endl; }
-    catch (const std::exception& e)
-        { cout << e.what() <<endl; failures.push_back("testGait2354WithController"); }  
-
-    if (!failures.empty()) {
-        cout << "Done, with failure(s): " << failures << endl;
-        return 1;
-    }
-
-    cout << "Done" << endl;
-    return 0;
+    SimTK_START_TEST("testForward");
+        // test manager/integration process
+        SimTK_SUBTEST(testPendulum);
+        // test application of external loads point in pendulum
+        SimTK_SUBTEST(testPendulumExternalLoad);
+        // test application of external loads with point moving in ground
+        SimTK_SUBTEST(testPendulumExternalLoadWithPointInGround);
+        // now add computation of controls and generation of muscle forces
+        SimTK_SUBTEST(testArm26);
+        // controlled muscles and ground reactions forces
+        SimTK_SUBTEST(testGait2354);
+        // included additional controller
+        SimTK_SUBTEST(testGait2354WithController);
+        // implements steps GUI takes to provide a model
+        SimTK_SUBTEST(testGait2354WithControllerGUI);
+    SimTK_END_TEST();
 }
 
 void testPendulum() {
@@ -249,3 +223,39 @@ void testGait2354WithController() {
     CHECK_STORAGE_AGAINST_STANDARD(results, *standard, rms_tols,
         __FILE__, __LINE__, "testGait2354WithController failed");
 }
+
+void testGait2354WithControllerGUI() {
+
+    // The following lines are the steps from ForwardToolModel.java that
+    // associates the a previous (orgiModel) model with the ForwardTool
+    // instead of the Tool loading the model specified in the setup
+    ForwardTool forward("subject01_Setup_Forward_Controller.xml");
+    Model origModel(forward.getModelFilename());
+
+    Model* model = new Model(origModel);
+    model->initSystem();
+    
+    const std::string resultsDir{ "ResultsCorrectionControllerGUI" };
+
+    forward.setResultsDir(resultsDir);
+    forward.updateModelForces(*model, "");
+    forward.setModel(*model);
+
+    model->initSystem();
+
+    forward.run();
+
+    // For good measure we'll make sure we still get the identical results
+    Storage results(resultsDir+"/subject01_states.sto");
+    //Storage standard("std_subject01_walk1_states.sto");
+    Storage standard("ResultsCorrectionController/subject01_states.sto");
+
+    int nstates = forward.getModel().getNumStateVariables();
+    int nq = forward.getModel().getNumCoordinates();
+    std::vector<double> rms_tols(2 * nstates, SimTK::SqrtEps);
+
+    CHECK_STORAGE_AGAINST_STANDARD(results, standard, rms_tols,
+        __FILE__, __LINE__, "testGait2354WithControllerGUI failed");
+    
+    delete model;
+}

OpenSim/Tools/CorrectionController.cpp

@@ -223,29 +223,27 @@ void CorrectionController::computeControls(const SimTK::State& s, SimTK::Vector&
 
     SimTK::Vector actControls(1, 0.0);
 
-    for(int i=0; i< getActuatorSet().getSize(); i++){
-        auto act = 
-            dynamic_cast<const CoordinateActuator*>(&getActuatorSet().get(i));
-        SimTK_ASSERT( act,  "CorrectionController::computeControls dynamic cast failed");
-
-        Coordinate *aCoord = act->getCoordinate();
-        if( aCoord->isConstrained(s) ) {
+    int i = 0;
+    auto coordinateActuators = getComponentList<CoordinateActuator>();
+    for(auto& act : coordinateActuators) {
+        const Coordinate* coord = act.getCoordinate();
+        if(coord->isConstrained(s) ) {
             actControls =  0.0;
         } 
-        else
-        {
-            double qval = aCoord->getValue(s);
-            double uval = aCoord->getSpeedValue(s);
+        else {
+            double qval = coord->getValue(s);
+            double uval = coord->getSpeedValue(s);
 
             // COMPUTE EXCITATIONS
-            double oneOverFmax = 1.0 / act->getOptimalForce();
+            double oneOverFmax = 1.0 / act.getOptimalForce();
             double pErr = qval - yDesired[2*i];
             double vErr = uval - yDesired[2*i+1];
             double pErrTerm = _kp*oneOverFmax*pErr;
             double vErrTerm = _kv*oneOverFmax*vErr;
             actControls = -vErrTerm - pErrTerm;
         }
-        getActuatorSet()[i].addInControls(actControls, controls);
+        act.addInControls(actControls, controls);
+        ++i;
     }
 }
 
@@ -257,16 +255,23 @@ void CorrectionController::extendConnectToModel(Model& model)
     // create an actuator for each generalized coordinate in the model 
     // add these actuators to the model and set their indexes 
     const CoordinateSet& cs = _model->getCoordinateSet();
+    auto actuators = model.updComponentList<CoordinateActuator>();
+
     for(int i=0; i<cs.getSize(); i++) {
-        std::cout << " CorrectionController::extendConnectToModel(): " 
-                  <<  cs.get(i).getName()+"_corrector" << "  added " 
-                  << std::endl;
-        std::string name = cs.get(i).getName()+"_corrector";
-        CoordinateActuator *actuator = NULL;
-        if(_model->getForceSet().contains(name)){
-            actuator = (CoordinateActuator *)&_model->getForceSet().get(name);
+        const Coordinate& coord = cs[i];
+        const std::string name = coord.getName() + "_corrector";
+
+        CoordinateActuator* actuator = nullptr;
+
+        for (auto& ca : actuators) {
+            if (ca.getName() == name) {
+                actuator = &ca;
+                break;
+            }
         }
-        else{
+
+        if(!actuator) {
+            // create the corrector actuator if it doe not already exist
             actuator = new CoordinateActuator();
             actuator->setCoordinate(&cs.get(i));
             actuator->setName(name);
@@ -275,16 +280,22 @@ void CorrectionController::extendConnectToModel(Model& model)
             // the controller is removed, so are all the actuators it added.
             adoptSubcomponent(actuator);
             setNextSubcomponentInSystem(*actuator);
-        }
 
-        actuator->setOptimalForce(1.0);
+            std::cout << " CorrectionController::extendConnectToModel(): "
+                << name << " added " << std::endl;
+
+            actuator->setOptimalForce(1.0);
+        }
 
-        updActuators().adoptAndAppend(actuator);
+        // Add to the Controller's list of Actuators (no ownership).
+        addActuator(*actuator);
    }
+
     setNumControls(getActuatorSet().getSize());
 
-    printf(" CorrectionController::extendConnectToModel()  num Actuators= %d kv=%f kp=%f \n",
-        _model->getForceSet().getSize(), _kv, _kp );
+    printf(" CorrectionController::extendConnectToModel() " 
+        "num Actuators= %d kv=%f kp=%f \n",
+        getNumControls(), _kv, _kp );
 }
 
 // for any initialization requiring a state or the complete system