From d86813354abe9ed8aec861e47b32a223426cfa69 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Wed, 22 Nov 2023 15:13:21 -0800 Subject: [PATCH 01/48] Outline for PrescribedController support in Moco --- .../Moco/Components/DiscreteController.cpp | 17 +++++- OpenSim/Moco/Components/DiscreteController.h | 1 + .../MocoCasADiSolver/MocoCasOCProblem.cpp | 27 +++----- .../Moco/MocoCasADiSolver/MocoCasOCProblem.h | 3 +- OpenSim/Moco/MocoGoal/MocoControlGoal.cpp | 1 + OpenSim/Moco/MocoProblemRep.cpp | 61 +++++++++++++++++-- OpenSim/Moco/MocoProblemRep.h | 10 +++ OpenSim/Moco/Test/testMocoInterface.cpp | 18 +++--- 8 files changed, 103 insertions(+), 35 deletions(-) diff --git a/OpenSim/Moco/Components/DiscreteController.cpp b/OpenSim/Moco/Components/DiscreteController.cpp index 5f7d44a4bf..5a42191873 100644 --- a/OpenSim/Moco/Components/DiscreteController.cpp +++ b/OpenSim/Moco/Components/DiscreteController.cpp @@ -49,14 +49,27 @@ void DiscreteController::computeControls( const auto& dv = subSys.getDiscreteVariable(s, m_discreteVarIndex) ; const auto& discreteControls = SimTK::Value::downcast(dv).get(); - controls += discreteControls; + + for (int i = 0; i < getActuatorSet().getSize(); ++i) { + controls[m_actuatorIndices[i]] += discreteControls[i]; + } } void DiscreteController::extendRealizeTopology(SimTK::State& state) const { Super::extendRealizeTopology(state); const SimTK::Subsystem& subSys = getSystem().getDefaultSubsystem(); + + int count = 0; + for (const auto& actu : getModel().getComponentList()) { + if (getActuatorSet().contains(actu.getName())) { + m_actuatorIndices.push_back(count); + } + ++count; + } + OPENSIM_ASSERT(getActuatorSet().getSize() == m_actuatorIndices.size()); + m_discreteVarIndex = subSys.allocateDiscreteVariable(state, SimTK::Stage::Dynamics, new SimTK::Value( - SimTK::Vector(getModel().getNumControls(), 0.0))); + SimTK::Vector(getActuatorSet().getSize(), 0.0))); } diff --git a/OpenSim/Moco/Components/DiscreteController.h b/OpenSim/Moco/Components/DiscreteController.h index 09f44464e0..5b5a4625dd 100644 --- a/OpenSim/Moco/Components/DiscreteController.h +++ b/OpenSim/Moco/Components/DiscreteController.h @@ -39,6 +39,7 @@ class DiscreteController : public Controller { protected: void extendRealizeTopology(SimTK::State&) const override; mutable SimTK::DiscreteVariableIndex m_discreteVarIndex; + mutable std::vector m_actuatorIndices; }; diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp index 80888e89a6..25445ea227 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp @@ -41,18 +41,6 @@ MocoCasOCProblem::MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver, setDynamicsMode(dynamicsMode); const auto& model = problemRep.getModelBase(); - // Ensure the model does not have user-provided controllers. - int numControllers = 0; - for (const auto& controller : model.getComponentList()) { - // Avoid unused variable warning. - (void)&controller; - ++numControllers; - } - // The model has a DiscreteController added by MocoProblemRep; any other - // controllers were added by the user. - OPENSIM_THROW_IF(numControllers > 1, Exception, - "MocoCasADiSolver does not support models with Controllers."); - if (problemRep.isPrescribedKinematics()) { setPrescribedKinematics(true, model.getWorkingState().getNU()); } @@ -75,13 +63,18 @@ MocoCasOCProblem::MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver, convertBounds(info.getFinalBounds())); } - auto controlNames = - createControlNamesFromModel(model, m_modelControlIndices); - for (const auto& controlName : controlNames) { - const auto& info = problemRep.getControlInfo(controlName); - addControl(controlName, convertBounds(info.getBounds()), + // Add control variables for the DiscreteController (i.e., controls that + // do not have a user-defined Controller associated with them). + const auto& controllerMap = problemRep.getControllerMap(); + m_modelControlIndices.clear(); + m_modelControlIndices.reserve( + controllerMap.at("discrete_controller").size()); + for (const auto& value : controllerMap.at("discrete_controller")) { + const auto& info = problemRep.getControlInfo(value.first); + addControl(value.first, convertBounds(info.getBounds()), convertBounds(info.getInitialBounds()), convertBounds(info.getFinalBounds())); + m_modelControlIndices.push_back(value.second); } // Set the number of residual equations to be enforced for components with diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h index 515203a46b..5a0a4a93c0 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h @@ -560,8 +560,7 @@ class MocoCasOCProblem : public CasOC::Problem { SimTK::Vector& simtkControls = discreteController.updDiscreteControls(simtkState); for (int ic = 0; ic < getNumControls(); ++ic) { - simtkControls[m_modelControlIndices[ic]] = - *(controls.ptr() + ic); + simtkControls[ic] = *(controls.ptr() + ic); } } } diff --git a/OpenSim/Moco/MocoGoal/MocoControlGoal.cpp b/OpenSim/Moco/MocoGoal/MocoControlGoal.cpp index 1b0201d128..cf2f65bd1e 100644 --- a/OpenSim/Moco/MocoGoal/MocoControlGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoControlGoal.cpp @@ -52,6 +52,7 @@ void MocoControlGoal::setWeightForControlPattern( void MocoControlGoal::initializeOnModelImpl(const Model& model) const { // Get all expected control names. + // TODO check for controllers auto controlNames = createControlNamesFromModel(model); // Check that the model controls are in the correct order. diff --git a/OpenSim/Moco/MocoProblemRep.cpp b/OpenSim/Moco/MocoProblemRep.cpp index 68e98c298e..67070139c0 100644 --- a/OpenSim/Moco/MocoProblemRep.cpp +++ b/OpenSim/Moco/MocoProblemRep.cpp @@ -29,6 +29,7 @@ #include #include +#include using namespace OpenSim; @@ -94,7 +95,55 @@ void MocoProblemRep::initialize() { } } + // Manage controllers. + // ------------------- + // Create "master" list of control names and indices. These will be divided + // up among the controllers. + std::vector modelControlIndices; + auto controlNames = + createControlNamesFromModel(m_model_base, modelControlIndices); + + // Assign controls to controllers added by the user. + for (const auto& controller : m_model_base.getComponentList()) { + // TODO support other controllers + if (!dynamic_cast(&controller)) { + OPENSIM_THROW(Exception, "TODO"); + } + + const auto& actuators = controller.getActuatorSet(); + std::vector> controllerActuators; + controllerActuators.reserve(actuators.getSize()); + for (int i = 0; i < actuators.getSize(); ++i) { + const auto& actu = actuators.get(i); + const auto& actuName = actu.getAbsolutePathString(); + auto it = std::find(controlNames.begin(), controlNames.end(), + actuName); + int index = modelControlIndices[it - controlNames.begin()]; + std::cout << "DEBUG user controller control: " << actuName << " " << index << std::endl; + controllerActuators.emplace_back(actuName, index); + modelControlIndices.erase(modelControlIndices.begin() + + (it - controlNames.begin())); + controlNames.erase(it); + } + + m_controller_map[controller.getName()] = controllerActuators; + } + + std::cout << "DEBUG controlNames.size(): " << controlNames.size() << std::endl; + + // Add the remaining controls to the DiscreteController. auto discreteControllerBaseUPtr = make_unique(); + discreteControllerBaseUPtr->setName("discrete_controller"); + std::vector> discreteControllerActuators; + for (int i = 0; i < controlNames.size(); ++i) { + discreteControllerBaseUPtr->addActuator( + m_model_base.getComponent(controlNames[i])); + discreteControllerActuators.emplace_back( + controlNames[i], modelControlIndices[i]); + } + m_controller_map[discreteControllerBaseUPtr->getName()] = + discreteControllerActuators; + m_discrete_controller_base.reset(discreteControllerBaseUPtr.get()); m_model_base.addController(discreteControllerBaseUPtr.release()); @@ -428,7 +477,6 @@ void MocoProblemRep::initialize() { // Control infos. // -------------- - auto controlNames = createControlNamesFromModel(m_model_base); for (int i = 0; i < ph0.getProperty_control_infos_pattern().size(); ++i) { const auto& pattern = ph0.get_control_infos_pattern(i).getName(); auto regexPattern = std::regex(pattern); @@ -445,8 +493,8 @@ void MocoProblemRep::initialize() { const auto& name = ph0.get_control_infos(i).getName(); auto it = std::find(controlNames.begin(), controlNames.end(), name); OPENSIM_THROW_IF(it == controlNames.end(), Exception, - "Control info provided for nonexistent or disabled actuator " - "'{}'.", + "Control info provided for nonexistent, disabled, or " + "controlled actuator '{}'.", name); } @@ -456,9 +504,14 @@ void MocoProblemRep::initialize() { } // Loop through all the actuators in the model and create control infos - // for the associated actuator control variables. + // for any remaining actuators in the DiscreteController without infos. for (const auto& actu : m_model_base.getComponentList()) { const std::string actuName = actu.getAbsolutePathString(); + // If this actuator is not in the DiscreteController, skip it. + if (std::find(controlNames.begin(), controlNames.end(), actuName) == + controlNames.end()) { + continue; + } if (actu.numControls() == 1) { // No control info exists; add one. if (m_control_infos.count(actuName) == 0) { diff --git a/OpenSim/Moco/MocoProblemRep.h b/OpenSim/Moco/MocoProblemRep.h index bd64ca49eb..6528793cef 100644 --- a/OpenSim/Moco/MocoProblemRep.h +++ b/OpenSim/Moco/MocoProblemRep.h @@ -321,6 +321,14 @@ class OSIMMOCO_API MocoProblemRep { getImplicitComponentReferencePtrs() const { return m_implicit_component_refs; } + + /// TODO + const + std::unordered_map>>& + getControllerMap() const { + return m_controller_map; + } + /// @} private: @@ -396,6 +404,8 @@ class OSIMMOCO_API MocoProblemRep { std::unordered_map m_state_infos; std::unordered_map m_control_infos; + std::unordered_map>> + m_controller_map; std::vector> m_parameters; std::vector> m_costs; diff --git a/OpenSim/Moco/Test/testMocoInterface.cpp b/OpenSim/Moco/Test/testMocoInterface.cpp index 11570f47fd..dbe0825c83 100644 --- a/OpenSim/Moco/Test/testMocoInterface.cpp +++ b/OpenSim/Moco/Test/testMocoInterface.cpp @@ -2230,26 +2230,24 @@ TEMPLATE_TEST_CASE("Locked coordinates ", "", Catch::Contains("Coordinate '/slider/position' is locked")); } -/* -TEMPLATE_TEST_CASE("Controllers in the model", "", - MocoCasADiSolver, MocoTropterSolver) { +TEST_CASE("Controllers in the model", "") { MocoStudy study; auto& problem = study.updProblem(); auto model = createSlidingMassModel(); - auto* controller = new PrescribedController(); - controller->addActuator(model->getComponent("actuator")); - controller->prescribeControlForActuator("actuator", new Constant(0.4)); - model->addController(controller); + // auto* controller = new PrescribedController(); + // controller->addActuator(model->getComponent("actuator")); + // controller->prescribeControlForActuator("actuator", new Constant(0.4)); + // model->addController(controller); problem.setModel(std::move(model)); problem.setTimeBounds(0, {0, 10}); problem.setStateInfo("/slider/position/value", {0, 1}, 0, 1); problem.setStateInfo("/slider/position/speed", {-100, 100}, 0, 0); problem.addGoal(); - auto& solver = study.initSolver(); - solver.set_num_mesh_points(20); + auto& solver = study.initCasADiSolver(); + solver.set_num_mesh_intervals(20); + solver.set_parallel(0); MocoSolution solution = study.solve(); std::cout << "DEBUG " << solution.getControl("/actuator") << std::endl; } -*/ From 22a96b18ef4bff3f240e68d6ebb161b921c49889 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Sat, 25 Nov 2023 09:52:19 -0800 Subject: [PATCH 02/48] Update TropterProblem with new DiscreteController logic --- OpenSim/Moco/tropter/TropterProblem.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/OpenSim/Moco/tropter/TropterProblem.h b/OpenSim/Moco/tropter/TropterProblem.h index 1ba0421385..2d961aed8f 100644 --- a/OpenSim/Moco/tropter/TropterProblem.h +++ b/OpenSim/Moco/tropter/TropterProblem.h @@ -342,7 +342,7 @@ class MocoTropterSolver::TropterProblemBase : public tropter::Problem { m_mocoProbRep.getDiscreteControllerDisabledConstraints() .updDiscreteControls(simTKStateDisabledConstraints); for (int ic = 0; ic < controls.size(); ++ic) { - osimControls[m_modelControlIndices[ic]] = controls[ic]; + osimControls[ic] = controls[ic]; } } From 6ce6fad6747fa2e1cbed156bc00f981f794b6d84 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 27 Nov 2023 12:48:06 -0800 Subject: [PATCH 03/48] Properly handle non-scalar actuators --- .../Moco/Components/DiscreteController.cpp | 22 +++-- OpenSim/Moco/Components/DiscreteController.h | 2 +- .../MocoCasADiSolver/MocoCasOCProblem.cpp | 11 +-- OpenSim/Moco/MocoProblemRep.cpp | 82 +++++++++---------- OpenSim/Moco/MocoProblemRep.h | 6 ++ OpenSim/Moco/tropter/TropterProblem.h | 4 +- 6 files changed, 62 insertions(+), 65 deletions(-) diff --git a/OpenSim/Moco/Components/DiscreteController.cpp b/OpenSim/Moco/Components/DiscreteController.cpp index 5a42191873..ffdc235534 100644 --- a/OpenSim/Moco/Components/DiscreteController.cpp +++ b/OpenSim/Moco/Components/DiscreteController.cpp @@ -49,9 +49,8 @@ void DiscreteController::computeControls( const auto& dv = subSys.getDiscreteVariable(s, m_discreteVarIndex) ; const auto& discreteControls = SimTK::Value::downcast(dv).get(); - - for (int i = 0; i < getActuatorSet().getSize(); ++i) { - controls[m_actuatorIndices[i]] += discreteControls[i]; + for (int i = 0; i < m_controlIndices.size(); ++i) { + controls[m_controlIndices[i]] += discreteControls[i]; } } @@ -59,17 +58,22 @@ void DiscreteController::extendRealizeTopology(SimTK::State& state) const { Super::extendRealizeTopology(state); const SimTK::Subsystem& subSys = getSystem().getDefaultSubsystem(); + // Get the control indexes for the actuators that are in the actuator set. + // We store the indexes this way so that they are in the same order as the + // controls in the model. int count = 0; for (const auto& actu : getModel().getComponentList()) { - if (getActuatorSet().contains(actu.getName())) { - m_actuatorIndices.push_back(count); + for (int i = 0; i < actu.numControls(); ++i) { + if (getActuatorSet().contains(actu.getName())) { + m_controlIndices.push_back(count); + } + ++count; } - ++count; } - OPENSIM_ASSERT(getActuatorSet().getSize() == m_actuatorIndices.size()); + const SimTK::Vector initControls( + static_cast(m_controlIndices.size()), 0.0); m_discreteVarIndex = subSys.allocateDiscreteVariable(state, SimTK::Stage::Dynamics, - new SimTK::Value( - SimTK::Vector(getActuatorSet().getSize(), 0.0))); + new SimTK::Value(initControls)); } diff --git a/OpenSim/Moco/Components/DiscreteController.h b/OpenSim/Moco/Components/DiscreteController.h index 5b5a4625dd..9f63e7d33d 100644 --- a/OpenSim/Moco/Components/DiscreteController.h +++ b/OpenSim/Moco/Components/DiscreteController.h @@ -39,7 +39,7 @@ class DiscreteController : public Controller { protected: void extendRealizeTopology(SimTK::State&) const override; mutable SimTK::DiscreteVariableIndex m_discreteVarIndex; - mutable std::vector m_actuatorIndices; + mutable std::vector m_controlIndices; }; diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp index 25445ea227..bcd19276b9 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp @@ -65,16 +65,11 @@ MocoCasOCProblem::MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver, // Add control variables for the DiscreteController (i.e., controls that // do not have a user-defined Controller associated with them). - const auto& controllerMap = problemRep.getControllerMap(); - m_modelControlIndices.clear(); - m_modelControlIndices.reserve( - controllerMap.at("discrete_controller").size()); - for (const auto& value : controllerMap.at("discrete_controller")) { - const auto& info = problemRep.getControlInfo(value.first); - addControl(value.first, convertBounds(info.getBounds()), + for (const auto& controlName : problemRep.getControlNames()) { + const auto& info = problemRep.getControlInfo(controlName); + addControl(controlName, convertBounds(info.getBounds()), convertBounds(info.getInitialBounds()), convertBounds(info.getFinalBounds())); - m_modelControlIndices.push_back(value.second); } // Set the number of residual equations to be enforced for components with diff --git a/OpenSim/Moco/MocoProblemRep.cpp b/OpenSim/Moco/MocoProblemRep.cpp index 67070139c0..02a7d905a9 100644 --- a/OpenSim/Moco/MocoProblemRep.cpp +++ b/OpenSim/Moco/MocoProblemRep.cpp @@ -97,52 +97,49 @@ void MocoProblemRep::initialize() { // Manage controllers. // ------------------- - // Create "master" list of control names and indices. These will be divided - // up among the controllers. - std::vector modelControlIndices; - auto controlNames = - createControlNamesFromModel(m_model_base, modelControlIndices); + // Get the list of actuators in order stored in the model. + std::vector actuatorPaths; + for (const auto& actu : m_model_base.getComponentList()) { + actuatorPaths.push_back(actu.getAbsolutePathString()); + } // Assign controls to controllers added by the user. for (const auto& controller : m_model_base.getComponentList()) { - // TODO support other controllers if (!dynamic_cast(&controller)) { - OPENSIM_THROW(Exception, "TODO"); + OPENSIM_THROW(Exception, "Moco only supports PrescribedController " + "components. Controller '{}' is of type " + "'{}'.", + controller.getAbsolutePathString(), + controller.getConcreteClassName()); } const auto& actuators = controller.getActuatorSet(); - std::vector> controllerActuators; - controllerActuators.reserve(actuators.getSize()); for (int i = 0; i < actuators.getSize(); ++i) { const auto& actu = actuators.get(i); - const auto& actuName = actu.getAbsolutePathString(); - auto it = std::find(controlNames.begin(), controlNames.end(), - actuName); - int index = modelControlIndices[it - controlNames.begin()]; - std::cout << "DEBUG user controller control: " << actuName << " " << index << std::endl; - controllerActuators.emplace_back(actuName, index); - modelControlIndices.erase(modelControlIndices.begin() + - (it - controlNames.begin())); - controlNames.erase(it); + const auto& actuPath = actu.getAbsolutePathString(); + auto it = std::find(actuatorPaths.begin(), actuatorPaths.end(), + actuPath); + actuatorPaths.erase(it); } - - m_controller_map[controller.getName()] = controllerActuators; } - std::cout << "DEBUG controlNames.size(): " << controlNames.size() << std::endl; - // Add the remaining controls to the DiscreteController. + // Add the remaining actuators to the DiscreteController. + m_control_names.clear(); auto discreteControllerBaseUPtr = make_unique(); discreteControllerBaseUPtr->setName("discrete_controller"); - std::vector> discreteControllerActuators; - for (int i = 0; i < controlNames.size(); ++i) { - discreteControllerBaseUPtr->addActuator( - m_model_base.getComponent(controlNames[i])); - discreteControllerActuators.emplace_back( - controlNames[i], modelControlIndices[i]); + for (int i = 0; i < actuatorPaths.size(); ++i) { + const auto& actu = m_model_base.getComponent(actuatorPaths[i]); + discreteControllerBaseUPtr->addActuator(actu); + if (actu.numControls() > 1) { + for (int idx = 0; idx < actu.numControls(); ++idx) { + m_control_names.push_back(actuatorPaths[i] + "_" + + std::to_string(idx)); + } + } else { + m_control_names.push_back(actuatorPaths[i]); + } } - m_controller_map[discreteControllerBaseUPtr->getName()] = - discreteControllerActuators; m_discrete_controller_base.reset(discreteControllerBaseUPtr.get()); m_model_base.addController(discreteControllerBaseUPtr.release()); @@ -480,19 +477,19 @@ void MocoProblemRep::initialize() { for (int i = 0; i < ph0.getProperty_control_infos_pattern().size(); ++i) { const auto& pattern = ph0.get_control_infos_pattern(i).getName(); auto regexPattern = std::regex(pattern); - for (int j = 0; j < (int)controlNames.size(); ++j) { - if (std::regex_match(controlNames[j], regexPattern)) { - m_control_infos[controlNames[j]] = + for (const auto& control_name : m_control_names) { + if (std::regex_match(control_name, regexPattern)) { + m_control_infos[control_name] = ph0.get_control_infos_pattern(i); - m_control_infos[controlNames[j]].setName(controlNames[j]); + m_control_infos[control_name].setName(control_name); } } } for (int i = 0; i < ph0.getProperty_control_infos().size(); ++i) { const auto& name = ph0.get_control_infos(i).getName(); - auto it = std::find(controlNames.begin(), controlNames.end(), name); - OPENSIM_THROW_IF(it == controlNames.end(), Exception, + auto it = std::find(m_control_names.begin(), m_control_names.end(), name); + OPENSIM_THROW_IF(it == m_control_names.end(), Exception, "Control info provided for nonexistent, disabled, or " "controlled actuator '{}'.", name); @@ -503,15 +500,12 @@ void MocoProblemRep::initialize() { m_control_infos[name] = ph0.get_control_infos(i); } - // Loop through all the actuators in the model and create control infos - // for any remaining actuators in the DiscreteController without infos. - for (const auto& actu : m_model_base.getComponentList()) { + // Loop through the actuators in the DiscreteController and create control + // info for any without infos. + const auto& dcActuators = m_discrete_controller_base->getActuatorSet(); + for (int i = 0; i < dcActuators.getSize(); ++i) { + const auto& actu = dcActuators.get(i); const std::string actuName = actu.getAbsolutePathString(); - // If this actuator is not in the DiscreteController, skip it. - if (std::find(controlNames.begin(), controlNames.end(), actuName) == - controlNames.end()) { - continue; - } if (actu.numControls() == 1) { // No control info exists; add one. if (m_control_infos.count(actuName) == 0) { diff --git a/OpenSim/Moco/MocoProblemRep.h b/OpenSim/Moco/MocoProblemRep.h index 6528793cef..92269f1be5 100644 --- a/OpenSim/Moco/MocoProblemRep.h +++ b/OpenSim/Moco/MocoProblemRep.h @@ -329,6 +329,11 @@ class OSIMMOCO_API MocoProblemRep { return m_controller_map; } + /// TODO control names for the DiscreteController, in model order. + const std::vector& getControlNames() const { + return m_control_names; + } + /// @} private: @@ -404,6 +409,7 @@ class OSIMMOCO_API MocoProblemRep { std::unordered_map m_state_infos; std::unordered_map m_control_infos; + std::vector m_control_names; std::unordered_map>> m_controller_map; diff --git a/OpenSim/Moco/tropter/TropterProblem.h b/OpenSim/Moco/tropter/TropterProblem.h index 2d961aed8f..bb8dc9126e 100644 --- a/OpenSim/Moco/tropter/TropterProblem.h +++ b/OpenSim/Moco/tropter/TropterProblem.h @@ -108,9 +108,7 @@ class MocoTropterSolver::TropterProblemBase : public tropter::Problem { } void addControlVariables() { - auto controlNames = - createControlNamesFromModel(m_modelBase, m_modelControlIndices); - for (const auto& controlName : controlNames) { + for (const auto& controlName : m_mocoProbRep.getControlNames()) { const auto& info = m_mocoProbRep.getControlInfo(controlName); this->add_control(controlName, convertBounds(info.getBounds()), convertBounds(info.getInitialBounds()), From df78e9b0ee6fddc2f1674da7c1ec92c418ee3898 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 27 Nov 2023 16:37:23 -0800 Subject: [PATCH 04/48] Update utilities functions for controls to account for user-added Controllers --- .../MocoCasADiSolver/MocoCasOCProblem.cpp | 3 +- OpenSim/Moco/MocoControlBoundConstraint.cpp | 9 +- OpenSim/Moco/MocoControlBoundConstraint.h | 4 + OpenSim/Moco/MocoGoal/MocoControlGoal.cpp | 5 +- OpenSim/Moco/MocoGoal/MocoControlGoal.h | 4 + .../Moco/MocoGoal/MocoControlTrackingGoal.cpp | 12 ++- .../Moco/MocoGoal/MocoControlTrackingGoal.h | 4 + .../MocoGoal/MocoInitialActivationGoal.cpp | 12 ++- .../Moco/MocoGoal/MocoInitialActivationGoal.h | 4 + OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.cpp | 6 +- OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.h | 4 + OpenSim/Moco/MocoProblem.cpp | 2 +- OpenSim/Moco/MocoProblemRep.cpp | 94 +++++++++---------- OpenSim/Moco/MocoProblemRep.h | 17 ---- OpenSim/Moco/Test/testMocoInterface.cpp | 10 +- OpenSim/Moco/tropter/TropterProblem.h | 18 +--- .../MocoUserControlCost.cpp | 4 +- OpenSim/Simulation/SimulationUtilities.cpp | 57 +++++++++-- OpenSim/Simulation/SimulationUtilities.h | 20 +++- 19 files changed, 173 insertions(+), 116 deletions(-) diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp index bcd19276b9..4deccee33a 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp @@ -65,7 +65,8 @@ MocoCasOCProblem::MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver, // Add control variables for the DiscreteController (i.e., controls that // do not have a user-defined Controller associated with them). - for (const auto& controlName : problemRep.getControlNames()) { + auto controlNames = createControlNamesFromModel(model, true, true); + for (const auto& controlName : controlNames) { const auto& info = problemRep.getControlInfo(controlName); addControl(controlName, convertBounds(info.getBounds()), convertBounds(info.getInitialBounds()), diff --git a/OpenSim/Moco/MocoControlBoundConstraint.cpp b/OpenSim/Moco/MocoControlBoundConstraint.cpp index 03df7deea7..ce022b37f6 100644 --- a/OpenSim/Moco/MocoControlBoundConstraint.cpp +++ b/OpenSim/Moco/MocoControlBoundConstraint.cpp @@ -39,9 +39,6 @@ void MocoControlBoundConstraint::constructProperties() { void MocoControlBoundConstraint::initializeOnModelImpl( const Model& model, const MocoProblemInfo& problemInfo) const { - // Get all expected control names. - auto controlNames = createControlNamesFromModel(model); - // Check that the model controls are in the correct order. checkOrderSystemControls(model); @@ -53,13 +50,15 @@ void MocoControlBoundConstraint::initializeOnModelImpl( } // Make sure there are no nonexistent controls. if (m_hasLower || m_hasUpper) { - auto systemControlIndexMap = createSystemControlIndexMap(model); + auto systemControlIndexMap = + createSystemControlIndexMap(model, true, true); for (int i = 0; i < getProperty_control_paths().size(); ++i) { const auto& thisName = get_control_paths(i); OPENSIM_THROW_IF_FRMOBJ(systemControlIndexMap.count(thisName) == 0, Exception, "Control path '{}' was provided but no such " - "control exists in the model.", + "control exists in the model or it is already controlled " + "by a user-defined controller.", thisName); m_controlIndices.push_back(systemControlIndexMap[thisName]); } diff --git a/OpenSim/Moco/MocoControlBoundConstraint.h b/OpenSim/Moco/MocoControlBoundConstraint.h index 032bb6c76d..a24a00bc9e 100644 --- a/OpenSim/Moco/MocoControlBoundConstraint.h +++ b/OpenSim/Moco/MocoControlBoundConstraint.h @@ -38,6 +38,10 @@ not perform such a check for other types of functions. constrain any control signals, even if you have provided control paths. @note This class can only constrain control signals for ScalarActuator%s. + +@note Controls belonging to actuators controlled by user-defined controllers +will not appear in the MocoProblem, and therefore cannot be constrained by +this class. @ingroup mocopathcon */ class OSIMMOCO_API MocoControlBoundConstraint : public MocoPathConstraint { OpenSim_DECLARE_CONCRETE_OBJECT( diff --git a/OpenSim/Moco/MocoGoal/MocoControlGoal.cpp b/OpenSim/Moco/MocoGoal/MocoControlGoal.cpp index cf2f65bd1e..aa3f69e95b 100644 --- a/OpenSim/Moco/MocoGoal/MocoControlGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoControlGoal.cpp @@ -52,13 +52,12 @@ void MocoControlGoal::setWeightForControlPattern( void MocoControlGoal::initializeOnModelImpl(const Model& model) const { // Get all expected control names. - // TODO check for controllers - auto controlNames = createControlNamesFromModel(model); + auto controlNames = createControlNamesFromModel(model, true, true); // Check that the model controls are in the correct order. checkOrderSystemControls(model); - auto systemControlIndexMap = createSystemControlIndexMap(model); + auto systemControlIndexMap = createSystemControlIndexMap(model, true, true); // Make sure there are no weights for nonexistent controls. for (int i = 0; i < get_control_weights().getSize(); ++i) { const auto& thisName = get_control_weights()[i].getName(); diff --git a/OpenSim/Moco/MocoGoal/MocoControlGoal.h b/OpenSim/Moco/MocoGoal/MocoControlGoal.h index 35981bfc4a..3949dd6508 100644 --- a/OpenSim/Moco/MocoGoal/MocoControlGoal.h +++ b/OpenSim/Moco/MocoGoal/MocoControlGoal.h @@ -49,6 +49,10 @@ We use the following notation: If `p > 2`, we first take the absolute value of the control; this is to properly handle odd exponents. + +@note Controls belonging to actuators controlled by user-defined controllers +are not included in the cost. + @ingroup mocogoal */ class OSIMMOCO_API MocoControlGoal : public MocoGoal { OpenSim_DECLARE_CONCRETE_OBJECT(MocoControlGoal, MocoGoal); diff --git a/OpenSim/Moco/MocoGoal/MocoControlTrackingGoal.cpp b/OpenSim/Moco/MocoGoal/MocoControlTrackingGoal.cpp index 8cd4cb24bd..603c1f574e 100644 --- a/OpenSim/Moco/MocoGoal/MocoControlTrackingGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoControlTrackingGoal.cpp @@ -66,8 +66,10 @@ void MocoControlTrackingGoal::addScaleFactor(const std::string &name, void MocoControlTrackingGoal::initializeOnModelImpl(const Model& model) const { // Get a map between control names and their indices in the model. This also - // checks that the model controls are in the correct order. - auto allControlIndices = createSystemControlIndexMap(model); + // checks that the model controls are in the correct order. Controls + // associated with actuators controlled by user-defined controllers are not + // included in this map. + auto allControlIndices = createSystemControlIndexMap(model, true, true); // Throw exception if a weight is specified for a nonexistent control. for (int i = 0; i < get_control_weights().getSize(); ++i) { @@ -75,7 +77,8 @@ void MocoControlTrackingGoal::initializeOnModelImpl(const Model& model) const { if (allControlIndices.count(weightName) == 0) { OPENSIM_THROW_FRMOBJ(Exception, "Weight provided with name '{}' but this is " - "not a recognized control.", + "not a recognized control or it is already controlled by a" + "user-defined controller.", weightName); } } @@ -92,7 +95,8 @@ void MocoControlTrackingGoal::initializeOnModelImpl(const Model& model) const { const auto& controlName = get_reference_labels(i).getName(); if (allControlIndices.count(controlName) == 0) { OPENSIM_THROW_FRMOBJ(Exception, - "Control '{}' does not exist in the model.", controlName); + "Control '{}' does not exist in the model or it is already " + "controlled by a user-defined controller.", controlName); } OPENSIM_THROW_IF_FRMOBJ(controlsToTrack.count(controlName), Exception, "Expected control '{}' to be provided no more " diff --git a/OpenSim/Moco/MocoGoal/MocoControlTrackingGoal.h b/OpenSim/Moco/MocoGoal/MocoControlTrackingGoal.h index 6d41054ea9..392093a28d 100644 --- a/OpenSim/Moco/MocoGoal/MocoControlTrackingGoal.h +++ b/OpenSim/Moco/MocoGoal/MocoControlTrackingGoal.h @@ -130,6 +130,10 @@ controlTrackingGoal->addScaleFactor( Tracking problems in direct collocation perform best when tracking smooth data, so it is recommended to filter the data in the reference you provide to the cost. + +@note Controls belonging to actuators controlled by user-defined controllers +cannot be tracked by this goal. + @ingroup mocogoal */ class OSIMMOCO_API MocoControlTrackingGoal : public MocoGoal { OpenSim_DECLARE_CONCRETE_OBJECT(MocoControlTrackingGoal, MocoGoal); diff --git a/OpenSim/Moco/MocoGoal/MocoInitialActivationGoal.cpp b/OpenSim/Moco/MocoGoal/MocoInitialActivationGoal.cpp index f7c947b43b..113f2b40cd 100644 --- a/OpenSim/Moco/MocoGoal/MocoInitialActivationGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoInitialActivationGoal.cpp @@ -25,12 +25,18 @@ using namespace OpenSim; void MocoInitialActivationGoal::initializeOnModelImpl( const Model& model) const { + // Get a map of all the state indices in the system. auto allSysYIndices = createSystemYIndexMap(model); - auto systemControlIndexMap = createSystemControlIndexMap(model); + + // Get a map of all the control indices in the system. Skip muscles that + // are already controlled by a user-defined controller. + auto systemControlIndexMap = createSystemControlIndexMap(model, true, true); for (const auto& muscle : model.getComponentList()) { - if (!muscle.get_ignore_activation_dynamics()) { - const std::string path = muscle.getAbsolutePathString(); + const std::string path = muscle.getAbsolutePathString(); + const bool muscleIsNotControlled = systemControlIndexMap.find(path) != + systemControlIndexMap.end(); + if (!muscle.get_ignore_activation_dynamics() && muscleIsNotControlled) { int excitationIndex = systemControlIndexMap[path]; int activationIndex = allSysYIndices[path + "/activation"]; m_indices.emplace_back(excitationIndex, activationIndex); diff --git a/OpenSim/Moco/MocoGoal/MocoInitialActivationGoal.h b/OpenSim/Moco/MocoGoal/MocoInitialActivationGoal.h index 12d9dfdd48..b7380df71b 100644 --- a/OpenSim/Moco/MocoGoal/MocoInitialActivationGoal.h +++ b/OpenSim/Moco/MocoGoal/MocoInitialActivationGoal.h @@ -30,6 +30,10 @@ possible value in inverse/tracking problems which penalize only excitations This is an endpoint constraint goal by default. Credit for using this goal to address excessive initial activation goes to Jessica Allen. + +@note Muscles that are already controlled by a user-defined controller are +ignored by this goal. + @ingroup mocogoal */ class OSIMMOCO_API MocoInitialActivationGoal : public MocoGoal { OpenSim_DECLARE_CONCRETE_OBJECT(MocoInitialActivationGoal, MocoGoal); diff --git a/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.cpp b/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.cpp index a7af089e15..0c1acfddf0 100644 --- a/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.cpp @@ -78,16 +78,16 @@ void MocoPeriodicityGoal::initializeOnModelImpl(const Model& model) const { get_state_pairs(i).get_negate() ? -1 : 1); } - auto systemControlIndexMap = createSystemControlIndexMap(model); + auto systemControlIndexMap = createSystemControlIndexMap(model, true, true); int nControlPairs = getProperty_control_pairs().size(); for (int i = 0; i < nControlPairs; ++i) { const auto path1 = get_control_pairs(i).get_initial_variable(); OPENSIM_THROW_IF(systemControlIndexMap.count(path1) == 0, Exception, - "Could not find control '{}'.", path1); + "Could not find control variable '{}'.", path1); const auto path2 = get_control_pairs(i).get_final_variable(); OPENSIM_THROW_IF(systemControlIndexMap.count(path2) == 0, Exception, - "Could not find control '{}'.", path2); + "Could not find control variable '{}'.", path2); int controlIndex1 = systemControlIndexMap[path1]; int controlIndex2 = systemControlIndexMap[path2]; m_control_names.emplace_back(path1, path2); diff --git a/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.h b/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.h index 6e7faec96e..dbb39c1b78 100644 --- a/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.h +++ b/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.h @@ -95,6 +95,10 @@ pair_hamstrings2.set_final_variable("/hamstrings_r"); periodicGoal->append_control_pairs(pair_hamstrings2); @endcode This is an endpoint constraint goal by default. + +@note Controls belonging to actuators controlled by user-defined controllers are +ignored by this goal. + @ingroup mocogoal */ class OSIMMOCO_API MocoPeriodicityGoal : public MocoGoal { OpenSim_DECLARE_CONCRETE_OBJECT(MocoPeriodicityGoal, MocoGoal); diff --git a/OpenSim/Moco/MocoProblem.cpp b/OpenSim/Moco/MocoProblem.cpp index c3503f9783..a72070735f 100644 --- a/OpenSim/Moco/MocoProblem.cpp +++ b/OpenSim/Moco/MocoProblem.cpp @@ -108,7 +108,7 @@ void MocoPhase::printControlNamesWithSubstring(const std::string& substring) { std::vector foundNames; Model model = get_model().process(); model.initSystem(); - const auto controlNames = createControlNamesFromModel(model); + const auto controlNames = createControlNamesFromModel(model, true, true); for (int i = 0; i < (int)controlNames.size(); ++i) { if (controlNames[i].find(substring) != std::string::npos) { foundNames.push_back(controlNames[i]); diff --git a/OpenSim/Moco/MocoProblemRep.cpp b/OpenSim/Moco/MocoProblemRep.cpp index 02a7d905a9..9205003e61 100644 --- a/OpenSim/Moco/MocoProblemRep.cpp +++ b/OpenSim/Moco/MocoProblemRep.cpp @@ -97,13 +97,7 @@ void MocoProblemRep::initialize() { // Manage controllers. // ------------------- - // Get the list of actuators in order stored in the model. - std::vector actuatorPaths; - for (const auto& actu : m_model_base.getComponentList()) { - actuatorPaths.push_back(actu.getAbsolutePathString()); - } - - // Assign controls to controllers added by the user. + // Check that the any controllers added by the user are valid. for (const auto& controller : m_model_base.getComponentList()) { if (!dynamic_cast(&controller)) { OPENSIM_THROW(Exception, "Moco only supports PrescribedController " @@ -112,33 +106,27 @@ void MocoProblemRep::initialize() { controller.getAbsolutePathString(), controller.getConcreteClassName()); } - - const auto& actuators = controller.getActuatorSet(); - for (int i = 0; i < actuators.getSize(); ++i) { - const auto& actu = actuators.get(i); - const auto& actuPath = actu.getAbsolutePathString(); - auto it = std::find(actuatorPaths.begin(), actuatorPaths.end(), - actuPath); - actuatorPaths.erase(it); - } } + // Get the list of actuators that are controlled by the user. There should + // be no DiscreteController in the model at this point, so we set the second + // argument to false. + std::vector controlledActuatorPaths = + createControlledActuatorPathsFromModel(m_model_base, false); // Add the remaining actuators to the DiscreteController. - m_control_names.clear(); auto discreteControllerBaseUPtr = make_unique(); discreteControllerBaseUPtr->setName("discrete_controller"); - for (int i = 0; i < actuatorPaths.size(); ++i) { - const auto& actu = m_model_base.getComponent(actuatorPaths[i]); - discreteControllerBaseUPtr->addActuator(actu); - if (actu.numControls() > 1) { - for (int idx = 0; idx < actu.numControls(); ++idx) { - m_control_names.push_back(actuatorPaths[i] + "_" + - std::to_string(idx)); - } - } else { - m_control_names.push_back(actuatorPaths[i]); + for (const auto& actu : m_model_base.getComponentList()) { + if (std::find(controlledActuatorPaths.begin(), + controlledActuatorPaths.end(), + actu.getAbsolutePathString()) != + controlledActuatorPaths.end()) { + continue; } + if (!actu.get_appliesForce()) { continue; } + + discreteControllerBaseUPtr->addActuator(actu); } m_discrete_controller_base.reset(discreteControllerBaseUPtr.get()); @@ -474,22 +462,23 @@ void MocoProblemRep::initialize() { // Control infos. // -------------- + std::vector controlNames = createControlNamesFromModel( + m_model_base, true, true); for (int i = 0; i < ph0.getProperty_control_infos_pattern().size(); ++i) { const auto& pattern = ph0.get_control_infos_pattern(i).getName(); auto regexPattern = std::regex(pattern); - for (const auto& control_name : m_control_names) { - if (std::regex_match(control_name, regexPattern)) { - m_control_infos[control_name] = - ph0.get_control_infos_pattern(i); - m_control_infos[control_name].setName(control_name); + for (const auto& controlName : controlNames) { + if (std::regex_match(controlName, regexPattern)) { + m_control_infos[controlName] = ph0.get_control_infos_pattern(i); + m_control_infos[controlName].setName(controlName); } } } for (int i = 0; i < ph0.getProperty_control_infos().size(); ++i) { const auto& name = ph0.get_control_infos(i).getName(); - auto it = std::find(m_control_names.begin(), m_control_names.end(), name); - OPENSIM_THROW_IF(it == m_control_names.end(), Exception, + auto it = std::find(controlNames.begin(), controlNames.end(), name); + OPENSIM_THROW_IF(it == controlNames.end(), Exception, "Control info provided for nonexistent, disabled, or " "controlled actuator '{}'.", name); @@ -526,19 +515,6 @@ void MocoProblemRep::initialize() { MocoBounds::unconstrained()); } } - - if (ph0.get_bound_activation_from_excitation()) { - const auto* muscle = dynamic_cast(&actu); - if (muscle && !muscle->get_ignore_activation_dynamics()) { - const std::string stateName = actuName + "/activation"; - auto& info = m_state_infos[stateName]; - if (info.getName().empty()) { info.setName(stateName); } - if (!info.getBounds().isSet()) { - info.setBounds(m_control_infos[actuName].getBounds()); - } - } - } - } else { // This is a non-scalar actuator, so we need to add multiple // control infos. @@ -556,6 +532,30 @@ void MocoProblemRep::initialize() { } } + // Bound activations based on muscle excitation control bounds bounds. Set + // this for all muscles, including those controller by a user-defined + // controller. + if (ph0.get_bound_activation_from_excitation()) { + for (const auto& actu : m_model_base.getComponentList()) { + const std::string actuName = actu.getAbsolutePathString(); + const auto* muscle = dynamic_cast(&actu); + if (muscle && !muscle->get_ignore_activation_dynamics()) { + const std::string stateName = actuName + "/activation"; + auto& info = m_state_infos[stateName]; + if (info.getName().empty()) { info.setName(stateName); } + if (!info.getBounds().isSet()) { + if (m_control_infos.count(actuName)) { + info.setBounds(m_control_infos[actuName].getBounds()); + } else { + info.setBounds( + {muscle->getMinControl(), muscle->getMaxControl()}); + } + + } + } + } + } + // Auxiliary state implicit residual outputs. const auto allImplicitResiduals = getModelOutputReferencePtrs( m_model_disabled_constraints, "^implicitresidual_.*", true); diff --git a/OpenSim/Moco/MocoProblemRep.h b/OpenSim/Moco/MocoProblemRep.h index 92269f1be5..c00ef5d1db 100644 --- a/OpenSim/Moco/MocoProblemRep.h +++ b/OpenSim/Moco/MocoProblemRep.h @@ -322,20 +322,6 @@ class OSIMMOCO_API MocoProblemRep { return m_implicit_component_refs; } - /// TODO - const - std::unordered_map>>& - getControllerMap() const { - return m_controller_map; - } - - /// TODO control names for the DiscreteController, in model order. - const std::vector& getControlNames() const { - return m_control_names; - } - - /// @} - private: explicit MocoProblemRep(const MocoProblem& problem); friend MocoProblem; @@ -409,9 +395,6 @@ class OSIMMOCO_API MocoProblemRep { std::unordered_map m_state_infos; std::unordered_map m_control_infos; - std::vector m_control_names; - std::unordered_map>> - m_controller_map; std::vector> m_parameters; std::vector> m_costs; diff --git a/OpenSim/Moco/Test/testMocoInterface.cpp b/OpenSim/Moco/Test/testMocoInterface.cpp index dbe0825c83..bf28323b31 100644 --- a/OpenSim/Moco/Test/testMocoInterface.cpp +++ b/OpenSim/Moco/Test/testMocoInterface.cpp @@ -2234,14 +2234,14 @@ TEST_CASE("Controllers in the model", "") { MocoStudy study; auto& problem = study.updProblem(); auto model = createSlidingMassModel(); - // auto* controller = new PrescribedController(); - // controller->addActuator(model->getComponent("actuator")); - // controller->prescribeControlForActuator("actuator", new Constant(0.4)); - // model->addController(controller); + auto* controller = new PrescribedController(); + controller->addActuator(model->getComponent("actuator")); + controller->prescribeControlForActuator("actuator", new Constant(0.4)); + model->addController(controller); problem.setModel(std::move(model)); problem.setTimeBounds(0, {0, 10}); problem.setStateInfo("/slider/position/value", {0, 1}, 0, 1); - problem.setStateInfo("/slider/position/speed", {-100, 100}, 0, 0); + problem.setStateInfo("/slider/position/speed", {-100, 100}, 0); problem.addGoal(); auto& solver = study.initCasADiSolver(); diff --git a/OpenSim/Moco/tropter/TropterProblem.h b/OpenSim/Moco/tropter/TropterProblem.h index bb8dc9126e..4ed5796251 100644 --- a/OpenSim/Moco/tropter/TropterProblem.h +++ b/OpenSim/Moco/tropter/TropterProblem.h @@ -63,19 +63,6 @@ class MocoTropterSolver::TropterProblemBase : public tropter::Problem { m_stateBase), Exception, "Quaternions are not supported."); - // Ensure the model does not have user-provided controllers. - int numControllers = 0; - for (const auto& controller : - m_modelBase.template getComponentList()) { - // Avoid unused variable warning. - (void)&controller; - ++numControllers; - } - // The model has a DiscreteController added by MocoProblemRep; any other - // controllers were added by the user. - OPENSIM_THROW_IF(numControllers > 1, Exception, - "MocoCasADiSolver does not support models with Controllers."); - // It is sufficient to create these containers from the original model // since the discrete variables added to the model with disabled // constraints wouldn't show up anyway. @@ -108,7 +95,10 @@ class MocoTropterSolver::TropterProblemBase : public tropter::Problem { } void addControlVariables() { - for (const auto& controlName : m_mocoProbRep.getControlNames()) { + // Add control variables for the DiscreteController (i.e., controls that + // do not have a user-defined Controller associated with them). + auto controlNames = createControlNamesFromModel(m_modelBase, true, true); + for (const auto& controlName : controlNames) { const auto& info = m_mocoProbRep.getControlInfo(controlName); this->add_control(controlName, convertBounds(info.getBounds()), convertBounds(info.getInitialBounds()), diff --git a/OpenSim/Sandbox/Moco/sandboxMocoUserControlCost/MocoUserControlCost.cpp b/OpenSim/Sandbox/Moco/sandboxMocoUserControlCost/MocoUserControlCost.cpp index b89523fda2..1eb5b89780 100644 --- a/OpenSim/Sandbox/Moco/sandboxMocoUserControlCost/MocoUserControlCost.cpp +++ b/OpenSim/Sandbox/Moco/sandboxMocoUserControlCost/MocoUserControlCost.cpp @@ -45,12 +45,12 @@ void MocoUserControlCost::setWeight( void MocoUserControlCost::initializeOnModelImpl(const Model& model) const { // Get all expected control names. - auto controlNames = createControlNamesFromModel(model); + auto controlNames = createControlNamesFromModel(model, true, true); // Check that the model controls are in the correct order. checkOrderSystemControls(model); - auto systemControlIndexMap = createSystemControlIndexMap(model); + auto systemControlIndexMap = createSystemControlIndexMap(model, true, true); // Make sure there are no weights for nonexistent controls. for (int i = 0; i < get_control_weights().getSize(); ++i) { const auto& thisName = get_control_weights()[i].getName(); diff --git a/OpenSim/Simulation/SimulationUtilities.cpp b/OpenSim/Simulation/SimulationUtilities.cpp index 4b1bbb3b54..cd639dfeff 100644 --- a/OpenSim/Simulation/SimulationUtilities.cpp +++ b/OpenSim/Simulation/SimulationUtilities.cpp @@ -32,6 +32,7 @@ #include #include #include +#include "OpenSim/Moco/Components/DiscreteController.h" using namespace OpenSim; @@ -305,21 +306,48 @@ std::unordered_map OpenSim::createSystemYIndexMap( return sysYIndices; } +std::vector OpenSim::createControlledActuatorPathsFromModel( + const Model& model, const bool ignoreDiscreteController) { + std::vector actuPaths; + for (const auto& controller : model.getComponentList()) { + if (dynamic_cast(&controller) && + ignoreDiscreteController) { + continue; + } + const auto& actuators = controller.getActuatorSet(); + for (int i = 0; i < actuators.getSize(); ++i) { + const auto& actu = actuators.get(i); + actuPaths.push_back(actu.getAbsolutePathString()); + } + } + + return actuPaths; +} + std::vector OpenSim::createControlNamesFromModel( - const Model& model, std::vector& modelControlIndices) { + const Model& model, std::vector& modelControlIndices, + const bool skipControlledActuators, + const bool ignoreDiscreteController) { std::vector controlNames; + std::vector controlledActuPaths = + createControlledActuatorPathsFromModel(model, ignoreDiscreteController); + // Loop through all actuators and create control names. For scalar // actuators, use the actuator name for the control name. For non-scalar // actuators, use the actuator name with a control index appended for the // control name. - // TODO update when OpenSim supports named controls. int count = 0; modelControlIndices.clear(); for (const auto& actu : model.getComponentList()) { - if (!actu.get_appliesForce()) { + const bool isControlledActuator = + std::find(controlledActuPaths.begin(), controlledActuPaths.end(), + actu.getAbsolutePathString()) != controlledActuPaths.end(); + if (!actu.get_appliesForce() || + (skipControlledActuators && isControlledActuator)) { count += actu.numControls(); continue; } + std::string actuPath = actu.getAbsolutePathString(); if (actu.numControls() == 1) { controlNames.push_back(actuPath); @@ -336,14 +364,19 @@ std::vector OpenSim::createControlNamesFromModel( return controlNames; } + std::vector OpenSim::createControlNamesFromModel( - const Model& model) { + const Model& model, + const bool skipControlledActuators, + const bool ignoreDiscreteController) { std::vector modelControlIndices; - return createControlNamesFromModel(model, modelControlIndices); + return createControlNamesFromModel(model, modelControlIndices, + skipControlledActuators, ignoreDiscreteController); } std::unordered_map OpenSim::createSystemControlIndexMap( - const Model& model) { + const Model& model, const bool skipControlledActuators, + const bool ignoreDiscreteController) { // We often assume that control indices in the state are in the same order // as the actuators in the model. However, the control indices are // allocated in the order in which addToSystem() is invoked (not @@ -351,8 +384,9 @@ std::unordered_map OpenSim::createSystemControlIndexMap( // absolutely sure that the controls are in the same order as actuators, // we can run the following check: in order, set an actuator's control // signal(s) to NaN and ensure the i-th control is NaN. - // TODO update when OpenSim supports named controls. std::unordered_map controlIndices; + std::vector controlledActuPaths = + createControlledActuatorPathsFromModel(model, ignoreDiscreteController); const SimTK::State state = model.getWorkingState(); auto modelControls = model.updControls(state); int i = 0; @@ -363,10 +397,17 @@ std::unordered_map OpenSim::createSystemControlIndexMap( actu.getControls(modelControls, origControls); actu.setControls(nan, modelControls); std::string actuPath = actu.getAbsolutePathString(); + const bool isControlledActuator = + std::find(controlledActuPaths.begin(), controlledActuPaths.end(), + actu.getAbsolutePathString()) != controlledActuPaths.end(); for (int j = 0; j < nc; ++j) { OPENSIM_THROW_IF(!SimTK::isNaN(modelControls[i]), Exception, "Internal error: actuators are not in the " "expected order. Submit a bug report."); + if (skipControlledActuators && isControlledActuator) { + ++i; + continue; + } if (nc == 1) { controlIndices[actuPath] = i; } else { @@ -380,7 +421,7 @@ std::unordered_map OpenSim::createSystemControlIndexMap( } void OpenSim::checkOrderSystemControls(const Model& model) { - createSystemControlIndexMap(model); + createSystemControlIndexMap(model, false, false); } void OpenSim::checkLabelsMatchModelStates(const Model& model, diff --git a/OpenSim/Simulation/SimulationUtilities.h b/OpenSim/Simulation/SimulationUtilities.h index 2dd9fb52c7..7af832fa0a 100644 --- a/OpenSim/Simulation/SimulationUtilities.h +++ b/OpenSim/Simulation/SimulationUtilities.h @@ -152,6 +152,11 @@ OSIMSIMULATION_API std::unordered_map createSystemYIndexMap(const Model& model); #endif +/// TODO +OSIMSIMULATION_API +std::vector createControlledActuatorPathsFromModel( + const Model& model, bool ignoreDiscreteController = false); + /// Create a vector of control names based on the actuators in the model for /// which appliesForce == True. For actuators with one control (e.g. /// ScalarActuator) the control name is simply the actuator name. For actuators @@ -161,13 +166,19 @@ std::unordered_map createSystemYIndexMap(const Model& model); /// (appliesForce == True). Its elements are the indices of the controls in the /// Model::updControls() that are associated with actuators that apply a force. /// @ingroup simulationutil +/// TODO ingoring controllers OSIMSIMULATION_API std::vector createControlNamesFromModel( - const Model& model, std::vector& modelControlIndices); + const Model& model, std::vector& modelControlIndices, + bool skipControlledActuators = false, + bool ignoreDiscreteController = false); /// Same as above, but when there is no mapping to the modelControlIndices. /// @ingroup simulationutil +/// TODO ingoring controllers OSIMSIMULATION_API -std::vector createControlNamesFromModel(const Model& model); +std::vector createControlNamesFromModel(const Model& model, + bool skipControlledActuators = false, + bool ignoreDiscreteController = false); /// The map provides the index of each control variable in the SimTK::Vector /// returned by Model::getControls(), using the control name as the /// key. @@ -176,9 +187,12 @@ std::vector createControlNamesFromModel(const Model& model); /// error, but you may be able to avoid the error by ensuring all Actuator%s /// are in the Model's ForceSet. /// @ingroup simulationutil +/// TODO ingoring controllers OSIMSIMULATION_API std::unordered_map createSystemControlIndexMap( - const Model& model); + const Model& model, + bool skipControlledActuators = false, + bool ignoreDiscreteController = false); /// Throws an exception if the order of the controls in the model is not the /// same as the order of the actuators in the model. From 31ea193c7956f2c9c74a32cab9385348b9ac711a Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Tue, 28 Nov 2023 11:08:05 -0800 Subject: [PATCH 05/48] Add sliding mass w/ PrescribedController test --- .../MocoCasADiSolver/MocoCasADiSolver.cpp | 10 +-- OpenSim/Moco/MocoProblemRep.cpp | 12 ++-- OpenSim/Moco/Test/testMocoInterface.cpp | 67 ++++++++++++++----- 3 files changed, 60 insertions(+), 29 deletions(-) diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp index 82e9550190..7cbf69b792 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp @@ -377,11 +377,11 @@ MocoSolution MocoCasADiSolver::solveImpl() const { Logger::Level origLoggerLevel = Logger::getLevel(); Logger::setLevel(Logger::Level::Warn); CasOC::Solution casSolution; - try { - casSolution = casSolver->solve(casGuess); - } catch (...) { - OpenSim::Logger::setLevel(origLoggerLevel); - } + // try { + casSolution = casSolver->solve(casGuess); + // } catch (...) { + // OpenSim::Logger::setLevel(origLoggerLevel); + // } OpenSim::Logger::setLevel(origLoggerLevel); MocoSolution mocoSolution = diff --git a/OpenSim/Moco/MocoProblemRep.cpp b/OpenSim/Moco/MocoProblemRep.cpp index 9205003e61..978a8e652c 100644 --- a/OpenSim/Moco/MocoProblemRep.cpp +++ b/OpenSim/Moco/MocoProblemRep.cpp @@ -114,17 +114,17 @@ void MocoProblemRep::initialize() { std::vector controlledActuatorPaths = createControlledActuatorPathsFromModel(m_model_base, false); - // Add the remaining actuators to the DiscreteController. + // Add the non-controlled, enabled actuators to the DiscreteController. auto discreteControllerBaseUPtr = make_unique(); discreteControllerBaseUPtr->setName("discrete_controller"); for (const auto& actu : m_model_base.getComponentList()) { - if (std::find(controlledActuatorPaths.begin(), - controlledActuatorPaths.end(), - actu.getAbsolutePathString()) != - controlledActuatorPaths.end()) { + bool isControlled = std::find(controlledActuatorPaths.begin(), + controlledActuatorPaths.end(), + actu.getAbsolutePathString()) != + controlledActuatorPaths.end(); + if (isControlled || !actu.get_appliesForce()) { continue; } - if (!actu.get_appliesForce()) { continue; } discreteControllerBaseUPtr->addActuator(actu); } diff --git a/OpenSim/Moco/Test/testMocoInterface.cpp b/OpenSim/Moco/Test/testMocoInterface.cpp index bf28323b31..9033b2b266 100644 --- a/OpenSim/Moco/Test/testMocoInterface.cpp +++ b/OpenSim/Moco/Test/testMocoInterface.cpp @@ -2230,24 +2230,55 @@ TEMPLATE_TEST_CASE("Locked coordinates ", "", Catch::Contains("Coordinate '/slider/position' is locked")); } -TEST_CASE("Controllers in the model", "") { - MocoStudy study; - auto& problem = study.updProblem(); - auto model = createSlidingMassModel(); - auto* controller = new PrescribedController(); - controller->addActuator(model->getComponent("actuator")); - controller->prescribeControlForActuator("actuator", new Constant(0.4)); - model->addController(controller); - problem.setModel(std::move(model)); - problem.setTimeBounds(0, {0, 10}); - problem.setStateInfo("/slider/position/value", {0, 1}, 0, 1); - problem.setStateInfo("/slider/position/speed", {-100, 100}, 0); - problem.addGoal(); +TEMPLATE_TEST_CASE("Sliding mass with PrescribedController", "", + MocoCasADiSolver, MocoTropterSolver) { - auto& solver = study.initCasADiSolver(); - solver.set_num_mesh_intervals(20); - solver.set_parallel(0); - MocoSolution solution = study.solve(); - std::cout << "DEBUG " << solution.getControl("/actuator") << std::endl; + // Solve a sliding mass problem and store the results. + TimeSeriesTable controlsTable; + SimTK::Matrix statesTrajectory; + { + MocoStudy study; + auto& problem = study.updProblem(); + auto model = createSlidingMassModel(); + problem.setModel(std::move(model)); + problem.setTimeBounds(0, 5); + problem.setStateInfo("/slider/position/value", {0, 1}, 0, 1); + problem.setStateInfo("/slider/position/speed", {-100, 100}, 0, 0); + problem.setControlInfo("/actuator", {-50, 50}); + problem.addGoal(); + auto& solver = study.initSolver(); + solver.set_num_mesh_intervals(50); + MocoSolution solution = study.solve(); + statesTrajectory = solution.getStatesTrajectory(); + controlsTable = solution.exportToControlsTable(); + } + + // Apply the control from the previous problem to a new problem with a + // PrescribedController and check that we get the same states trajectory + // back. + { + const auto& time = controlsTable.getIndependentColumn(); + const auto& control = controlsTable.getDependentColumn("/actuator"); + auto model = createSlidingMassModel(); + auto* controller = new PrescribedController(); + controller->addActuator(model->getComponent("/actuator")); + controller->prescribeControlForActuator("actuator", + new GCVSpline(5, control.size(), time.data(), &control[0], + "/actuator", 0.0)); + model->addController(controller); + model->finalizeConnections(); + MocoStudy study; + auto& problem = study.updProblem(); + problem.setModel(std::move(model)); + problem.setTimeBounds(0, 5); + problem.setStateInfo("/slider/position/value", {0, 1}, 0); + problem.setStateInfo("/slider/position/speed", {-100, 100}, 0); + auto& solver = study.initSolver(); + solver.set_num_mesh_intervals(50); + MocoSolution solution = study.solve(); + + OpenSim_REQUIRE_MATRIX_ABSTOL(solution.getStatesTrajectory(), + statesTrajectory, 1e-9); + } } From c0a857eb6158c62239a452df1ac6cb3e581e2ec2 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Tue, 28 Nov 2023 13:17:19 -0800 Subject: [PATCH 06/48] Use bracket include --- OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp | 10 +++++----- OpenSim/Simulation/SimulationUtilities.cpp | 2 +- 2 files changed, 6 insertions(+), 6 deletions(-) diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp index 7cbf69b792..82e9550190 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp @@ -377,11 +377,11 @@ MocoSolution MocoCasADiSolver::solveImpl() const { Logger::Level origLoggerLevel = Logger::getLevel(); Logger::setLevel(Logger::Level::Warn); CasOC::Solution casSolution; - // try { - casSolution = casSolver->solve(casGuess); - // } catch (...) { - // OpenSim::Logger::setLevel(origLoggerLevel); - // } + try { + casSolution = casSolver->solve(casGuess); + } catch (...) { + OpenSim::Logger::setLevel(origLoggerLevel); + } OpenSim::Logger::setLevel(origLoggerLevel); MocoSolution mocoSolution = diff --git a/OpenSim/Simulation/SimulationUtilities.cpp b/OpenSim/Simulation/SimulationUtilities.cpp index cd639dfeff..c06a44c2d6 100644 --- a/OpenSim/Simulation/SimulationUtilities.cpp +++ b/OpenSim/Simulation/SimulationUtilities.cpp @@ -32,7 +32,7 @@ #include #include #include -#include "OpenSim/Moco/Components/DiscreteController.h" +#include using namespace OpenSim; From cc3a4854bbe12340caef0bccfa1a608bc5a2d332 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Tue, 28 Nov 2023 14:46:36 -0800 Subject: [PATCH 07/48] Move DiscreteController to Simulation library --- OpenSim/Moco/CMakeLists.txt | 2 -- OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h | 2 +- OpenSim/Moco/MocoProblemRep.cpp | 2 +- OpenSim/Moco/tropter/TropterProblem.h | 2 +- .../Control}/DiscreteController.cpp | 0 .../Components => Simulation/Control}/DiscreteController.h | 7 ++----- OpenSim/Simulation/SimulationUtilities.cpp | 2 +- 7 files changed, 6 insertions(+), 11 deletions(-) rename OpenSim/{Moco/Components => Simulation/Control}/DiscreteController.cpp (100%) rename OpenSim/{Moco/Components => Simulation/Control}/DiscreteController.h (94%) diff --git a/OpenSim/Moco/CMakeLists.txt b/OpenSim/Moco/CMakeLists.txt index 2d23eb4c5f..580daf9064 100644 --- a/OpenSim/Moco/CMakeLists.txt +++ b/OpenSim/Moco/CMakeLists.txt @@ -82,8 +82,6 @@ set(MOCO_SOURCES MocoFrameDistanceConstraint.cpp MocoOutputConstraint.h MocoOutputConstraint.cpp - Components/DiscreteController.cpp - Components/DiscreteController.h Components/StationPlaneContactForce.h Components/StationPlaneContactForce.cpp Components/DiscreteForces.h diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h index 5a0a4a93c0..26dffdfeae 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h @@ -21,8 +21,8 @@ #include "CasOCProblem.h" #include "MocoCasADiSolver.h" +#include #include -#include #include #include #include diff --git a/OpenSim/Moco/MocoProblemRep.cpp b/OpenSim/Moco/MocoProblemRep.cpp index 978a8e652c..26968c6019 100644 --- a/OpenSim/Moco/MocoProblemRep.cpp +++ b/OpenSim/Moco/MocoProblemRep.cpp @@ -19,7 +19,6 @@ #include "MocoProblemRep.h" #include "Components/AccelerationMotion.h" -#include "Components/DiscreteController.h" #include "Components/DiscreteForces.h" #include "MocoProblem.h" #include "MocoProblemInfo.h" @@ -29,6 +28,7 @@ #include #include +#include #include using namespace OpenSim; diff --git a/OpenSim/Moco/tropter/TropterProblem.h b/OpenSim/Moco/tropter/TropterProblem.h index 4ed5796251..d8ec789ac8 100644 --- a/OpenSim/Moco/tropter/TropterProblem.h +++ b/OpenSim/Moco/tropter/TropterProblem.h @@ -21,13 +21,13 @@ #include #include -#include #include #include #include #include #include #include +#include #include diff --git a/OpenSim/Moco/Components/DiscreteController.cpp b/OpenSim/Simulation/Control/DiscreteController.cpp similarity index 100% rename from OpenSim/Moco/Components/DiscreteController.cpp rename to OpenSim/Simulation/Control/DiscreteController.cpp diff --git a/OpenSim/Moco/Components/DiscreteController.h b/OpenSim/Simulation/Control/DiscreteController.h similarity index 94% rename from OpenSim/Moco/Components/DiscreteController.h rename to OpenSim/Simulation/Control/DiscreteController.h index 9f63e7d33d..096082069a 100644 --- a/OpenSim/Moco/Components/DiscreteController.h +++ b/OpenSim/Simulation/Control/DiscreteController.h @@ -18,15 +18,13 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ -#include - -#include +#include "Controller.h" namespace OpenSim { /** This component is used internally by Moco for passing a solver's control variables to a Model. */ -class DiscreteController : public Controller { +class OSIMSIMULATION_API DiscreteController : public Controller { OpenSim_DECLARE_CONCRETE_OBJECT(DiscreteController, Controller); public: DiscreteController() = default; @@ -45,5 +43,4 @@ class DiscreteController : public Controller { } // namespace OpenSim - #endif // OPENSIM_DISCRETECONTROLLER_H diff --git a/OpenSim/Simulation/SimulationUtilities.cpp b/OpenSim/Simulation/SimulationUtilities.cpp index c06a44c2d6..b968b2c438 100644 --- a/OpenSim/Simulation/SimulationUtilities.cpp +++ b/OpenSim/Simulation/SimulationUtilities.cpp @@ -32,7 +32,7 @@ #include #include #include -#include +#include using namespace OpenSim; From 157034a854975f0ebb5fec97b3dd4cdf34bde6c3 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Tue, 28 Nov 2023 14:47:30 -0800 Subject: [PATCH 08/48] Ignore .idea/editor.xml --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 76d89b05d1..411dcca9d1 100644 --- a/.gitignore +++ b/.gitignore @@ -53,6 +53,7 @@ delete_this_to_stop_optimization*.txt .idea/misc.xml .idea/codeStyles/codeStyleConfig.xml .idea/codeStyles/Project.xml +.idea/editor.xml # Sensitive or high-churn files: .idea/**/dataSources/ From a5059d7414364ed27a18e365303d245fadcd99dd Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Tue, 28 Nov 2023 15:29:52 -0800 Subject: [PATCH 09/48] Update docs for skipping controller actuators in SimulationUtilities --- .../Simulation/Control/DiscreteController.cpp | 2 +- OpenSim/Simulation/SimulationUtilities.h | 18 +++++++++++++----- 2 files changed, 14 insertions(+), 6 deletions(-) diff --git a/OpenSim/Simulation/Control/DiscreteController.cpp b/OpenSim/Simulation/Control/DiscreteController.cpp index ffdc235534..103c37bb7c 100644 --- a/OpenSim/Simulation/Control/DiscreteController.cpp +++ b/OpenSim/Simulation/Control/DiscreteController.cpp @@ -49,7 +49,7 @@ void DiscreteController::computeControls( const auto& dv = subSys.getDiscreteVariable(s, m_discreteVarIndex) ; const auto& discreteControls = SimTK::Value::downcast(dv).get(); - for (int i = 0; i < m_controlIndices.size(); ++i) { + for (int i = 0; i < (int)m_controlIndices.size(); ++i) { controls[m_controlIndices[i]] += discreteControls[i]; } } diff --git a/OpenSim/Simulation/SimulationUtilities.h b/OpenSim/Simulation/SimulationUtilities.h index 7af832fa0a..06251b6f87 100644 --- a/OpenSim/Simulation/SimulationUtilities.h +++ b/OpenSim/Simulation/SimulationUtilities.h @@ -152,7 +152,10 @@ OSIMSIMULATION_API std::unordered_map createSystemYIndexMap(const Model& model); #endif -/// TODO +/// Create a vector of actuator paths in the model associated with a Controller. +/// The flag 'ignoreDiscreteController' is used to exclude any actuators +/// associated a DiscreteController, which is used internally by Moco to pass +/// control variables from a solver to a model. OSIMSIMULATION_API std::vector createControlledActuatorPathsFromModel( const Model& model, bool ignoreDiscreteController = false); @@ -165,8 +168,11 @@ std::vector createControlledActuatorPathsFromModel( /// to the number of controls associated with actuators that apply a force /// (appliesForce == True). Its elements are the indices of the controls in the /// Model::updControls() that are associated with actuators that apply a force. +/// Use 'skipControlledActuators' to exclude any actuators associated with a +/// Controller; 'ignoreDiscreteController' is used to ignore the logic +/// 'skipControlledActuators' for DiscreteController%s (see +/// createControlledActuatorPathsFromModel()). /// @ingroup simulationutil -/// TODO ingoring controllers OSIMSIMULATION_API std::vector createControlNamesFromModel( const Model& model, std::vector& modelControlIndices, @@ -174,20 +180,22 @@ std::vector createControlNamesFromModel( bool ignoreDiscreteController = false); /// Same as above, but when there is no mapping to the modelControlIndices. /// @ingroup simulationutil -/// TODO ingoring controllers OSIMSIMULATION_API std::vector createControlNamesFromModel(const Model& model, bool skipControlledActuators = false, bool ignoreDiscreteController = false); /// The map provides the index of each control variable in the SimTK::Vector /// returned by Model::getControls(), using the control name as the -/// key. +/// key. Use 'skipControlledActuators' to exclude any actuators associated with +/// a Controller; 'ignoreDiscreteController' is used to ignore the logic +/// 'skipControlledActuators' for DiscreteController%s (see +/// createControlledActuatorPathsFromModel()). +/// /// @throws Exception if the order of actuators in the model does not match /// the order of controls in Model::getControls(). This is an internal /// error, but you may be able to avoid the error by ensuring all Actuator%s /// are in the Model's ForceSet. /// @ingroup simulationutil -/// TODO ingoring controllers OSIMSIMULATION_API std::unordered_map createSystemControlIndexMap( const Model& model, From 2c2a3e991b320d7673a9d492a6ce0ee0f7d759f2 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Tue, 28 Nov 2023 16:55:05 -0800 Subject: [PATCH 10/48] Update logic for excluding actuators associated with DiscreteControllers --- OpenSim/Simulation/SimulationUtilities.cpp | 24 +++++++++++++++++++++- 1 file changed, 23 insertions(+), 1 deletion(-) diff --git a/OpenSim/Simulation/SimulationUtilities.cpp b/OpenSim/Simulation/SimulationUtilities.cpp index b968b2c438..2081e4d731 100644 --- a/OpenSim/Simulation/SimulationUtilities.cpp +++ b/OpenSim/Simulation/SimulationUtilities.cpp @@ -317,7 +317,29 @@ std::vector OpenSim::createControlledActuatorPathsFromModel( const auto& actuators = controller.getActuatorSet(); for (int i = 0; i < actuators.getSize(); ++i) { const auto& actu = actuators.get(i); - actuPaths.push_back(actu.getAbsolutePathString()); + const auto& actuPath = actu.getAbsolutePathString(); + if (std::find(actuPaths.begin(), actuPaths.end(), + actuPath) == actuPaths.end()) { + actuPaths.push_back(actuPath); + } + + } + } + + // Remove any actuators that are also controlled by a DiscreteController. + if (ignoreDiscreteController) { + for (const auto& controller : + model.getComponentList()) { + const auto& actuators = controller.getActuatorSet(); + for (int i = 0; i < actuators.getSize(); ++i) { + const auto& actu = actuators.get(i); + const auto& actuPath = actu.getAbsolutePathString(); + auto it = std::find(actuPaths.begin(), actuPaths.end(), + actuPath); + if (it != actuPaths.end()) { + actuPaths.erase(it); + } + } } } From e0484c6e941fb37cb53116457bbf79140a4b7d14 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Wed, 29 Nov 2023 11:37:50 -0800 Subject: [PATCH 11/48] Double check that controls in DiscreteController are in the correct order --- OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp | 9 +++++++-- OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h | 1 + OpenSim/Moco/MocoProblemRep.cpp | 14 ++++++-------- OpenSim/Moco/tropter/TropterProblem.h | 12 +++++++++--- OpenSim/Simulation/Control/DiscreteController.cpp | 11 +++++++---- OpenSim/Simulation/Control/DiscreteController.h | 6 +++++- 6 files changed, 35 insertions(+), 18 deletions(-) diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp index 4deccee33a..5cf3d66332 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp @@ -63,8 +63,13 @@ MocoCasOCProblem::MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver, convertBounds(info.getFinalBounds())); } - // Add control variables for the DiscreteController (i.e., controls that - // do not have a user-defined Controller associated with them). + // Add control variables to the problem (i.e., any controls for actuators + // in the model's DiscreteController). First, double that the system + // controls are in the same order as the model. + checkOrderSystemControls(model); + // Get the list of control names (based on actuator paths) in the order of + // the system controls, which is the order the DiscreteController expects. + // Add the controls to the problem in the same order. auto controlNames = createControlNamesFromModel(model, true, true); for (const auto& controlName : controlNames) { const auto& info = problemRep.getControlInfo(controlName); diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h index 26dffdfeae..b0a080f127 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h @@ -560,6 +560,7 @@ class MocoCasOCProblem : public CasOC::Problem { SimTK::Vector& simtkControls = discreteController.updDiscreteControls(simtkState); for (int ic = 0; ic < getNumControls(); ++ic) { + // simtkControls[m_modelControlIndices[ic]] = *(controls.ptr() + ic); simtkControls[ic] = *(controls.ptr() + ic); } } diff --git a/OpenSim/Moco/MocoProblemRep.cpp b/OpenSim/Moco/MocoProblemRep.cpp index 26968c6019..bd2de8450e 100644 --- a/OpenSim/Moco/MocoProblemRep.cpp +++ b/OpenSim/Moco/MocoProblemRep.cpp @@ -122,11 +122,9 @@ void MocoProblemRep::initialize() { controlledActuatorPaths.end(), actu.getAbsolutePathString()) != controlledActuatorPaths.end(); - if (isControlled || !actu.get_appliesForce()) { - continue; + if (!isControlled && actu.get_appliesForce()) { + discreteControllerBaseUPtr->addActuator(actu); } - - discreteControllerBaseUPtr->addActuator(actu); } m_discrete_controller_base.reset(discreteControllerBaseUPtr.get()); @@ -489,8 +487,9 @@ void MocoProblemRep::initialize() { m_control_infos[name] = ph0.get_control_infos(i); } - // Loop through the actuators in the DiscreteController and create control - // info for any without infos. + // Loop through the actuators in the DiscreteController since these are the + // only actuators that will have associated optimal control variables. + // Create control infos for actuators that do not have a any control info. const auto& dcActuators = m_discrete_controller_base->getActuatorSet(); for (int i = 0; i < dcActuators.getSize(); ++i) { const auto& actu = dcActuators.get(i); @@ -533,7 +532,7 @@ void MocoProblemRep::initialize() { } // Bound activations based on muscle excitation control bounds bounds. Set - // this for all muscles, including those controller by a user-defined + // this for all muscles, including those controlled by a user-defined // controller. if (ph0.get_bound_activation_from_excitation()) { for (const auto& actu : m_model_base.getComponentList()) { @@ -550,7 +549,6 @@ void MocoProblemRep::initialize() { info.setBounds( {muscle->getMinControl(), muscle->getMaxControl()}); } - } } } diff --git a/OpenSim/Moco/tropter/TropterProblem.h b/OpenSim/Moco/tropter/TropterProblem.h index d8ec789ac8..898c487d1a 100644 --- a/OpenSim/Moco/tropter/TropterProblem.h +++ b/OpenSim/Moco/tropter/TropterProblem.h @@ -95,9 +95,15 @@ class MocoTropterSolver::TropterProblemBase : public tropter::Problem { } void addControlVariables() { - // Add control variables for the DiscreteController (i.e., controls that - // do not have a user-defined Controller associated with them). - auto controlNames = createControlNamesFromModel(m_modelBase, true, true); + // Add control variables to the problem (i.e., any controls for + // actuators in the model's DiscreteController). First, double check + // that the system controls are in the same order as the model. + checkOrderSystemControls(m_modelBase); + // Get the list of control names (based on actuator paths) in the order + // of the system controls, which is the order the DiscreteController + // expects. Add the controls to the problem in the same order. + auto controlNames = + createControlNamesFromModel(m_modelBase, true, true); for (const auto& controlName : controlNames) { const auto& info = m_mocoProbRep.getControlInfo(controlName); this->add_control(controlName, convertBounds(info.getBounds()), diff --git a/OpenSim/Simulation/Control/DiscreteController.cpp b/OpenSim/Simulation/Control/DiscreteController.cpp index 103c37bb7c..b25763ac5b 100644 --- a/OpenSim/Simulation/Control/DiscreteController.cpp +++ b/OpenSim/Simulation/Control/DiscreteController.cpp @@ -18,6 +18,7 @@ #include "DiscreteController.h" +#include #include #include @@ -59,15 +60,17 @@ void DiscreteController::extendRealizeTopology(SimTK::State& state) const { const SimTK::Subsystem& subSys = getSystem().getDefaultSubsystem(); // Get the control indexes for the actuators that are in the actuator set. - // We store the indexes this way so that they are in the same order as the - // controls in the model. + // This logic stores the indexes in the order of actuators stored in the + // model, skipping over indexes for any actuators not included in the + // DiscreteController's actuator set. int count = 0; for (const auto& actu : getModel().getComponentList()) { - for (int i = 0; i < actu.numControls(); ++i) { + const int nc = actu.numControls(); + for (int ic = 0; ic < nc; ++ic) { if (getActuatorSet().contains(actu.getName())) { m_controlIndices.push_back(count); } - ++count; + count++; } } diff --git a/OpenSim/Simulation/Control/DiscreteController.h b/OpenSim/Simulation/Control/DiscreteController.h index 096082069a..ad2b34a693 100644 --- a/OpenSim/Simulation/Control/DiscreteController.h +++ b/OpenSim/Simulation/Control/DiscreteController.h @@ -23,7 +23,11 @@ namespace OpenSim { /** This component is used internally by Moco for passing a solver's control -variables to a Model. */ +variables to a Model. Use `Controller::addActuator()` to add the actuators +that will be controlled by this controller. Actuators can be added in any order, +but the control indexes for these actuators will be stored in the order used by +the model. +*/ class OSIMSIMULATION_API DiscreteController : public Controller { OpenSim_DECLARE_CONCRETE_OBJECT(DiscreteController, Controller); public: From 679f817e8116d2a38dc849b66c44575568c69e92 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 8 Dec 2023 11:33:51 -0800 Subject: [PATCH 12/48] Start refactoring crude first implementation: adding InputController and ActuatorInputController --- OpenSim/CMakeLists.txt | 2 +- OpenSim/Moco/MocoProblemRep.cpp | 25 ++- OpenSim/Sandbox/Moco/sandboxSandbox.cpp | 169 +++++++++++++++- .../Simulation/Control/InputController.cpp | 190 ++++++++++++++++++ OpenSim/Simulation/Control/InputController.h | 173 ++++++++++++++++ .../Control/PrescribedController.cpp | 4 +- .../Simulation/Control/PrescribedController.h | 2 +- .../RegisterTypes_osimSimulation.cpp | 3 + 8 files changed, 551 insertions(+), 17 deletions(-) create mode 100644 OpenSim/Simulation/Control/InputController.cpp create mode 100644 OpenSim/Simulation/Control/InputController.h diff --git a/OpenSim/CMakeLists.txt b/OpenSim/CMakeLists.txt index 8d53dedd8f..8ba2fcc16e 100644 --- a/OpenSim/CMakeLists.txt +++ b/OpenSim/CMakeLists.txt @@ -10,6 +10,6 @@ add_subdirectory(Moco) add_subdirectory(Examples) add_subdirectory(Tests) -#add_subdirectory(Sandbox) +add_subdirectory(Sandbox) install(FILES OpenSim.h DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/OpenSim") diff --git a/OpenSim/Moco/MocoProblemRep.cpp b/OpenSim/Moco/MocoProblemRep.cpp index bd2de8450e..8663eab3a5 100644 --- a/OpenSim/Moco/MocoProblemRep.cpp +++ b/OpenSim/Moco/MocoProblemRep.cpp @@ -97,7 +97,22 @@ void MocoProblemRep::initialize() { // Manage controllers. // ------------------- + // Check that the order of the controls in the system matches the order of + // the actuators in the model. + checkOrderSystemControls(m_model_base); + + // Steps: + // 1) Check that existing controllers are valid (PrescribedController and ActuatorController, for now) + // 2) Skip over actuators that have controllers (when the user does not want to add controls for these actuators) + // 3) Add a ControlDistributor + + // TODO need a MocoProblem option to allow adding OCP controls for + // that already have a controller. + // Check that the any controllers added by the user are valid. + bool addControlsForControlledActuators = false; + std::vector actuatorsToSkip; + std::vector controlNamesToAddToControlDistributor; // TODO better name for (const auto& controller : m_model_base.getComponentList()) { if (!dynamic_cast(&controller)) { OPENSIM_THROW(Exception, "Moco only supports PrescribedController " @@ -105,14 +120,20 @@ void MocoProblemRep::initialize() { "'{}'.", controller.getAbsolutePathString(), controller.getConcreteClassName()); + + if (!addControlsForControlledActuators) { + for (const auto& actu : controller.getActuatorSet()) { + actuatorsToSkip.push_back(actu.getAbsolutePathString()); + } + } } } // Get the list of actuators that are controlled by the user. There should // be no DiscreteController in the model at this point, so we set the second // argument to false. - std::vector controlledActuatorPaths = - createControlledActuatorPathsFromModel(m_model_base, false); + // std::vector controlledActuatorPaths = + // createControlledActuatorPathsFromModel(m_model_base, false); // Add the non-controlled, enabled actuators to the DiscreteController. auto discreteControllerBaseUPtr = make_unique(); diff --git a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp index d935990b59..9f79189fd2 100644 --- a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp +++ b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp @@ -19,19 +19,168 @@ // This file provides a way to easily prototype or test temporary snippets of // code during development. -#include +#include "OpenSim/Actuators/CoordinateActuator.h" +#include "OpenSim/Actuators/ModelFactory.h" +#include "OpenSim/Simulation/Control/DiscreteController.h" +#include + +#include using namespace OpenSim; +class ControlAllocator : public Component { + OpenSim_DECLARE_CONCRETE_OBJECT(ControlAllocator, Component); +public: + // LIST OUTPUT + OpenSim_DECLARE_LIST_OUTPUT( + controls, double, getControlForOutputChannel, SimTK::Stage::Dynamics); + + // CONSTRUCTION + ControlAllocator() = default; + ControlAllocator(const ControlAllocator&) = default; + ControlAllocator(ControlAllocator&&) = default; + ControlAllocator& operator=(const ControlAllocator&) = default; + ControlAllocator& operator=(ControlAllocator&&) = default; + + // METHODS + void addControl(const std::string& controlName) { + const int index = m_controlIndexMap.size(); + m_controlIndexMap[controlName] = index; + } + + void setControls(SimTK::State& s, + const SimTK::Vector& controls) const { + updControls(s) = controls; + } + + SimTK::Vector& updControls(SimTK::State& s) const { + const SimTK::Subsystem& subSys = getSystem().getDefaultSubsystem(); + auto& dv = subSys.updDiscreteVariable(s, m_discreteVarIndex); + auto& discreteControls = + SimTK::Value::updDowncast(dv).upd(); + return discreteControls; + } + + const SimTK::Vector& getControls( + const SimTK::State& s) const { + const SimTK::Subsystem& subSys = getSystem().getDefaultSubsystem(); + auto& dv = subSys.getDiscreteVariable(s, m_discreteVarIndex); + auto& discreteControls = SimTK::Value::downcast(dv).get(); + return discreteControls; + } + + double getControlForOutputChannel(const SimTK::State& s, + const std::string& channel) const { + return getControls(s)[m_controlIndexMap.at(channel)]; + } + +protected: + void extendRealizeTopology(SimTK::State& state) const { + Super::extendRealizeTopology(state); + const SimTK::Subsystem& subSys = getSystem().getDefaultSubsystem(); + + const SimTK::Vector initControls( + static_cast(m_controlIndexMap.size()), 0.0); + + m_discreteVarIndex = + subSys.allocateDiscreteVariable(state, SimTK::Stage::Dynamics, + new SimTK::Value(initControls)); + } + + + void extendFinalizeFromProperties() { + Super::extendFinalizeFromProperties(); + for (const auto& kv : m_controlIndexMap) { + updOutput("controls").addChannel(kv.first); + } + } + +private: + std::unordered_map m_controlIndexMap; + mutable SimTK::DiscreteVariableIndex m_discreteVarIndex; + +}; + +// + +// class InputController : public Controller { +// OpenSim_DECLARE_ABSTRACT_OBJECT(InputController, Controller); +// public: +// OpenSim_DECLARE_LIST_INPUT(controls, double, SimTK::Stage::Dynamics, "controls"); +// InputController() = default; +// }; +// +// class ActuatorController : public InputController { +// OpenSim_DECLARE_CONCRETE_OBJECT(ActuatorController, InputController); +// public: +// ActuatorController() = default; +// +// void computeControls(const SimTK::State& s, +// SimTK::Vector& controls) const { +// +// SimTK::Vector control(1, 0.0); +// for (int i = 0; i < getActuatorSet().getSize(); ++i) { +// const auto& actu = getActuatorSet().get(i); +// const unsigned index = m_aliasMap.at(actu.getName()); +// control[0] = getInput("controls").getValue(s, index); +// actu.addInControls(control, controls); +// } +// +// } +// +// protected: +// void extendFinalizeFromProperties() { +// Super::extendFinalizeFromProperties(); +// const auto& input = getInput("controls"); +// for (int i = 0; i < input.getNumConnectees(); ++i) { +// const auto& alias = input.getAlias(i); +// OPENSIM_THROW_IF(!getActuatorSet().contains(alias), Exception, +// "Actuator '{}' not found in the ActuatorController's " +// "ActuatorSet.", alias); +// m_aliasMap[alias] = i; +// } +// } +// +// private: +// std::unordered_map m_aliasMap; +// +// }; + int main() { - // TODO Logger::setLevel(Logger::Level::Debug); - MocoTrack track; - ModelProcessor modelProcessor("DeMers_mod_noarms_welds_4.0.osim"); - modelProcessor.append(ModOpReplaceMusclesWithDeGrooteFregly2016()); - modelProcessor.append(ModOpIgnoreTendonCompliance()); - track.setModel(modelProcessor); - track.setStatesReference({"r_SLD_mean_coords.sto"}); - track.set_allow_unused_references(true); - MocoSolution solution = track.solve(); + + Model model = ModelFactory::createDoublePendulum(); + model.initSystem(); + + ControlAllocator* distributor = new ControlAllocator; + distributor->setName("distributor"); + distributor->addControl("/tau0"); + distributor->addControl("/tau1"); + model.addComponent(distributor); + + ActuatorInputController* controller = new ActuatorInputController; + controller->setName("actuator_controller"); + controller->addActuator(model.getComponent("/tau0")); + controller->addActuator(model.getComponent("/tau1")); + controller->connectInput_controls( + distributor->getOutput("controls").getChannel("/tau0"), "/tau0"); + controller->connectInput_controls( + distributor->getOutput("controls").getChannel("/tau1"), "/tau1"); + model.addController(controller); + model.finalizeFromProperties(); + model.finalizeConnections(); + + SimTK::State state = model.initSystem(); + + SimTK::Vector newControls(2, 0.0); + newControls[0] = 1.0; + newControls[1] = 2.0; + distributor->setControls(state, newControls); + model.realizeDynamics(state); + + model.printSubcomponentInfo(); + + std::cout << "actuation tau0: " << model.getComponent("/tau0").getActuation(state) << std::endl; + std::cout << "actuation tau1: " << model.getComponent("/tau1").getActuation(state) << std::endl; + return EXIT_SUCCESS; } diff --git a/OpenSim/Simulation/Control/InputController.cpp b/OpenSim/Simulation/Control/InputController.cpp new file mode 100644 index 0000000000..2a3cf00497 --- /dev/null +++ b/OpenSim/Simulation/Control/InputController.cpp @@ -0,0 +1,190 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: InputController.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-2023 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 "InputController.h" + +//#include + +using namespace OpenSim; + +//============================================================================= +// InputController +//============================================================================= + +//============================================================================= +// CONSTRUCTOR(S) AND DESTRUCTOR +//============================================================================= +InputController::InputController() : Controller() {} + +InputController::~InputController() = default; + +InputController::InputController(const InputController& other) + : Controller(other) {} + +InputController& InputController::operator=(const InputController& other) + : Controller(other) { + return *this; +} + +InputController::InputController(InputController&& other) noexcept = default; + +InputController& +InputController::operator=(InputController&& other) noexcept = default; + +//============================================================================= +// METHODS +//============================================================================= +const std::vector& InputController::getControlNames() const { + return m_controlNames; +} + +const std::vector& InputController::getControlIndexes() const { + return m_controlIndexes; +} + +std::vector InputController::getInputChannelAliases() const { + std::vector aliases; + const auto& input = getInput("inputs"); + aliases.reserve(input.getNumConnectees()); + for (int i = 0; i < input.getNumConnectees(); ++i) { + aliases.push_back(input.getAlias(i)); + } + + return aliases; +} + +//============================================================================= +// MODEL COMPONENT INTERFACE +//============================================================================= +void InputController::extendConnectToModel(Model& model) override { + Super::extendConnectToModel(model); + + // Get a list of the control names and indexes for all actuators in the + // model. These two vectors serve as a mapping between control names + // model control cache indexes. + std::vector modelControlIndexes; + const std::vector modelControlNames = + createControlNamesFromModel(model, modelControlIndexes); + + // Based on the controller's ActuatorSet, store lists of the control + // names and their indexes in the model control cache. + m_controlNames.clear(); + m_controlIndexes.clear(); + const auto& actuSet = getActuatorSet(); + for (int i = 0; i < actuSet.getSize(); ++i) { + const auto& actu = actuSet.get(i); + if (actu.numControls() > 1) { + // Non-scalar actuator. + for (int j = 0; j < actu.numControls(); ++j) { + // Use the control name format based on + // SimulationUtiltiies::createControlNamesFromModel(). + m_controlNames.push_back( + fmt::format("{}_{}", actu.getAbsolutePathString(), j)); + auto it = std::find(modelControlNames.begin(), + modelControlNames.end(), m_controlNames.back()); + m_controlIndexes.push_back( + modelControlIndexes[it - modelControlNames.begin()]); + } + } else { + // Scalar actuator. + m_controlNames.push_back(actu.getAbsolutePathString()); + auto it = std::find(modelControlNames.begin(), + modelControlNames.end(), m_controlNames.back()); + m_controlIndexes.push_back( + modelControlIndexes[it - modelControlNames.begin()]); + } + } + + setNumControls(static_cast(m_controlNames.size())); +} + + +//============================================================================= +// ActuatorInputController +//============================================================================= + +//============================================================================= +// CONSTRUCTOR(S) AND DESTRUCTOR +//============================================================================= +ActuatorInputController::ActuatorInputController() : InputController() {} + +ActuatorInputController::~ActuatorInputController() = default; + +ActuatorInputController::ActuatorInputController( + const ActuatorInputController& other) : InputController(other) {} + +ActuatorInputController& ActuatorInputController::operator=( + const ActuatorInputController& other) : InputController(other) { + return *this; +} + +ActuatorInputController::ActuatorInputController( + ActuatorInputController&& other) noexcept = default; + +ActuatorInputController& ActuatorInputController::operator=( + ActuatorInputController&& other) noexcept = default; + +//============================================================================= +// CONTROLLER INTERFACE +//============================================================================= +void ActuatorInputController::computeControls(const SimTK::State& s, + SimTK::Vector& controls) const override { + const auto& input = getInput("inputs"); + for (int i = 0; i < input.getNumConnectees(); ++i) { + controls[m_controlIndexesInConnecteeOrder[i]] = input.getValue(s, i); + } +} + +//============================================================================= +// MODEL COMPONENT INTERFACE +//============================================================================= +void ActuatorInputController::extendConnectToModel(Model& model) override { + Super::extendConnectToModel(model); + + const auto& controlNames = getControlNames(); + const auto& controlIndexes = getControlIndexes(); + OPENSIM_ASSERT(getNumControls() == + static_cast(controlNames.size())) + + const auto& input = getInput("inputs"); + OPENSIM_THROW_IF(input.getNumConnectees() != getNumControls(), Exception, + "Expected the number of Input connectees ({}) to match the number " + "of actuators controls ({}), but they do not.", + input.getNumConnectees(), getNumControls()); + + // Use the Input channel aliases to map the Input connectee indexes to + // the corresponding control indexes in the model control cache. We + // expect the Input connectee aliases to match the names of the actuator + // controls in the controller's ActuatorSet. + m_controlIndexesInConnecteeOrder.clear(); + m_controlIndexesInConnecteeOrder.reserve(getNumControls()); + for (const auto& alias : getInputChannelAliases()) { + auto it = std::find(controlNames.begin(), controlNames.end(), alias); + OPENSIM_THROW_IF(it == controlNames.end(), Exception, + "Expected the Input alias '{}' to match a control name for an " + "actuator in the controller's ActuatorSet, but it does not.", + alias); + m_controlIndexesInConnecteeOrder.push_back( + controlIndexes[it - controlNames.begin()]); + } +} \ No newline at end of file diff --git a/OpenSim/Simulation/Control/InputController.h b/OpenSim/Simulation/Control/InputController.h new file mode 100644 index 0000000000..c6b42f534a --- /dev/null +++ b/OpenSim/Simulation/Control/InputController.h @@ -0,0 +1,173 @@ +#ifndef OPENSIM_INPUT_CONTROLLER_H +#define OPENSIM_INPUT_CONTROLLER_H +/* -------------------------------------------------------------------------- * + * OpenSim: InputController.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-2023 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 "Controller.h" + +#include +#include + +namespace OpenSim { + +/** + * InputController is a simple intermediate abstract class for a Controller that + * computes controls based on scalar values defined via a list Input. + * + * Since InputController is an abstract class, derived clases must still + * implement Controller's virtual computeControls() method. Additionally, it is + * up to the derived class to define how the scalar values from the Input are + * mapped to the controls for the actuators in the controller's ActuatorSet. + * + * InputController provides convenience methods for getting the names and + * indexes of the controls for the actuators in the controller's ActuatorSet, + * and for getting the aliases for the connected Input channels. Non-scalar + * actuators will have multiple controls, and therefore have multiple control + * names and indexes. Channel names and control information are only available + * after calling Model::finalizeConnections(). + * + * Actuator control names and indexes are based on the convention used by the + * utility function SimulationUtilities::createControlNamesFromModel(), which + * returns control names and indexes based on the order of the actuators stored + * in the model. However, we do not check if the order of the actuators stored + * in the model matches the order of the controls in the underlying system. Use + * the utility function SimulationUtilities::checkOrderSystemControls() to + * perform this check when using this controller. + */ +class OSIMSIMULATION_API InputController : public Controller { +OpenSim_DECLARE_ABSTRACT_OBJECT(InputController, Controller); + +public: +//============================================================================= +// INPUTS +//============================================================================= + OpenSim_DECLARE_LIST_INPUT( + controls, double, SimTK::Stage::Dynamics, "inputs"); + +//============================================================================= +// METHODS +//============================================================================= + + // CONSTRUCTION/DESCTRUCTION + InputController(); + ~InputController() override; + + InputController(const InputController& other); + InputController& operator=(const InputController& other); + + InputController(InputController&& other) noexcept; + InputController& operator=(InputController&& other) noexcept; + + // METHODS + /** + * Get the names of the controls for the actuators in the controller's + * ActuatorSet. + * + * For scalar actuators, these names are simply the paths of + * the actuators in the model. For non-scalar actuators, these names are + * actuator path plus an additional suffix representing the index of the + * control in the actuator's control vector (e.g., "/actuator_0"). + * + * @note Only valid after calling Model::finalizeConnections(). + * + * @note This does *not* check if the order of the actuators stored in the + * model matches the order of the controls in the underlying system. + * Use SimulationUtilities::checkOrderSystemControls() to perform + * this check. + */ + const std::vector& getControlNames() const; + + /** + * Get the model control indexes for the controls associated with the + * actuators in the controller's ActuatorSet. + * + * The control indexes are based on the order of the actuators stored in the + * model. Non-scalar actuators will have multiple controls, and therefore + * have multiple control indexes. The order of the returned indexes matches + * the order of the control names returned by getControlNames(). + * + * @note Only valid after calling Model::finalizeConnections(). + * + * @note This does *not* check if the order of the actuators stored in the + * model matches the order of the controls in the underlying system. + * Use SimulationUtilities::checkOrderSystemControls() to perform + * this check. + */ + const std::vector& getControlIndexes() const; + + /** + * Get the aliases for the Input channels that are connected to the + * controller's Input. The aliases are returned in the same order as the + * channels stored in the Input. + * + * @note Only valid after calling Model::finalizeConnections(). + */ + std::vector getInputChannelAliases() const; + +protected: + // MODEL COMPONENT INTERFACE + void extendConnectToModel(Model& model) override; + +private: + std::vector m_controlNames; + std::vector m_controlIndexes; +}; + +/** + * ActuatorInputController is the simplest concrete implementation of an + * InputController. + * + * It passes the scalar values from the controller's list Input to the actuators + * in the controller's ActuatorSet. The Input channel aliases must match the + * names of the actuator controls in the model, but they do not need to be in + * the same order. + */ +class OSIMSIMULATION_API ActuatorInputController : public InputController { + OpenSim_DECLARE_CONCRETE_OBJECT(ActuatorInputController, InputController); +public: + + // CONSTRUCTION/DESCTRUCTION + ActuatorInputController(); + ~ActuatorInputController() override; + + ActuatorInputController(const ActuatorInputController& other); + ActuatorInputController& operator=(const ActuatorInputController& other); + + ActuatorInputController(ActuatorInputController&& other) noexcept; + ActuatorInputController& operator=(ActuatorInputController&& other) noexcept; + + // CONTROLLER INTERFACE + void computeControls( + const SimTK::State& s, SimTK::Vector& controls) const override; + +protected: + // MODEL COMPONENT INTERFACE + void extendConnectToModel(Model& model) override; + +private: + std::vector m_controlIndexesInConnecteeOrder; +}; + +} // namespace OpenSim + +#endif // OPENSIM_INPUT_CONTROLLER_H diff --git a/OpenSim/Simulation/Control/PrescribedController.cpp b/OpenSim/Simulation/Control/PrescribedController.cpp index a699df7a0f..07ff97a860 100644 --- a/OpenSim/Simulation/Control/PrescribedController.cpp +++ b/OpenSim/Simulation/Control/PrescribedController.cpp @@ -72,9 +72,7 @@ PrescribedController:: /* * Destructor. */ -PrescribedController::~PrescribedController() -{ -} +PrescribedController::~PrescribedController() = default; /* * Set NULL values for all member variables. diff --git a/OpenSim/Simulation/Control/PrescribedController.h b/OpenSim/Simulation/Control/PrescribedController.h index 249e46b0a5..2e882499d8 100644 --- a/OpenSim/Simulation/Control/PrescribedController.h +++ b/OpenSim/Simulation/Control/PrescribedController.h @@ -84,7 +84,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(PrescribedController, Controller); int interpMethodType = 1); /** Destructor */ - virtual ~PrescribedController(); + ~PrescribedController() override; //-------------------------------------------------------------------------- // CONTROL diff --git a/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp b/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp index 983e5eb53e..ef2c14e7db 100644 --- a/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp +++ b/OpenSim/Simulation/RegisterTypes_osimSimulation.cpp @@ -90,6 +90,7 @@ #include "Control/ControlConstant.h" #include "Control/ControlLinear.h" #include "Control/PrescribedController.h" +#include "Control/InputController.h" #include "Wrap/PathWrap.h" #include "Wrap/PathWrapSet.h" @@ -134,6 +135,7 @@ #include "TableProcessor.h" #include "MarkersReference.h" #include "PositionMotion.h" +#include "Control/InputController.h" #include #include @@ -267,6 +269,7 @@ OSIMSIMULATION_API void RegisterTypes_osimSimulation() Object::registerType( ControlSetController() ); Object::registerType( PrescribedController() ); + Object::registerType( ActuatorInputController() ); Object::registerType( PathActuator() ); Object::registerType( ProbeSet() ); From 7a22f03136c95a6690e8981ab515738647886900 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 11 Dec 2023 09:04:51 -0800 Subject: [PATCH 13/48] Exploring support for list Sockets --- OpenSim/Moco/MocoProblemRep.cpp | 16 ++++----- OpenSim/Sandbox/Moco/sandboxSandbox.cpp | 16 ++++----- OpenSim/Simulation/Control/Controller.cpp | 35 ++++++++++++------- OpenSim/Simulation/Control/Controller.h | 22 ++++++++---- .../Simulation/Control/InputController.cpp | 27 ++++++++++---- 5 files changed, 74 insertions(+), 42 deletions(-) diff --git a/OpenSim/Moco/MocoProblemRep.cpp b/OpenSim/Moco/MocoProblemRep.cpp index 8663eab3a5..410c99bf31 100644 --- a/OpenSim/Moco/MocoProblemRep.cpp +++ b/OpenSim/Moco/MocoProblemRep.cpp @@ -104,7 +104,7 @@ void MocoProblemRep::initialize() { // Steps: // 1) Check that existing controllers are valid (PrescribedController and ActuatorController, for now) // 2) Skip over actuators that have controllers (when the user does not want to add controls for these actuators) - // 3) Add a ControlDistributor + // 3) Add a ControlAllocator (replace DiscreteController) // TODO need a MocoProblem option to allow adding OCP controls for // that already have a controller. @@ -121,19 +121,19 @@ void MocoProblemRep::initialize() { controller.getAbsolutePathString(), controller.getConcreteClassName()); - if (!addControlsForControlledActuators) { - for (const auto& actu : controller.getActuatorSet()) { - actuatorsToSkip.push_back(actu.getAbsolutePathString()); - } - } + // if (!addControlsForControlledActuators) { + // for (const auto& actu : controller.getActuatorSet()) { + // actuatorsToSkip.push_back(actu.getAbsolutePathString()); + // } + // } } } // Get the list of actuators that are controlled by the user. There should // be no DiscreteController in the model at this point, so we set the second // argument to false. - // std::vector controlledActuatorPaths = - // createControlledActuatorPathsFromModel(m_model_base, false); + std::vector controlledActuatorPaths = + createControlledActuatorPathsFromModel(m_model_base, false); // Add the non-controlled, enabled actuators to the DiscreteController. auto discreteControllerBaseUPtr = make_unique(); diff --git a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp index 9f79189fd2..858378c25c 100644 --- a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp +++ b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp @@ -151,20 +151,20 @@ int main() { Model model = ModelFactory::createDoublePendulum(); model.initSystem(); - ControlAllocator* distributor = new ControlAllocator; - distributor->setName("distributor"); - distributor->addControl("/tau0"); - distributor->addControl("/tau1"); - model.addComponent(distributor); + ControlAllocator* allocator = new ControlAllocator; + allocator->setName("control_allocator"); + allocator->addControl("/tau0"); + allocator->addControl("/tau1"); + model.addComponent(allocator); ActuatorInputController* controller = new ActuatorInputController; controller->setName("actuator_controller"); controller->addActuator(model.getComponent("/tau0")); controller->addActuator(model.getComponent("/tau1")); controller->connectInput_controls( - distributor->getOutput("controls").getChannel("/tau0"), "/tau0"); + allocator->getOutput("controls").getChannel("/tau0"), "/tau0"); controller->connectInput_controls( - distributor->getOutput("controls").getChannel("/tau1"), "/tau1"); + allocator->getOutput("controls").getChannel("/tau1"), "/tau1"); model.addController(controller); model.finalizeFromProperties(); model.finalizeConnections(); @@ -174,7 +174,7 @@ int main() { SimTK::Vector newControls(2, 0.0); newControls[0] = 1.0; newControls[1] = 2.0; - distributor->setControls(state, newControls); + allocator->setControls(state, newControls); model.realizeDynamics(state); model.printSubcomponentInfo(); diff --git a/OpenSim/Simulation/Control/Controller.cpp b/OpenSim/Simulation/Control/Controller.cpp index 72a27b1741..943016420f 100644 --- a/OpenSim/Simulation/Control/Controller.cpp +++ b/OpenSim/Simulation/Control/Controller.cpp @@ -96,7 +96,7 @@ void Controller::constructProperties() setAuthors("Ajay Seth, Frank Anderson, Chand John, Samuel Hamner"); constructProperty_enabled(true); constructProperty_actuator_list(); - _actuatorSet.setMemoryOwner(false); + constructProperty_actuators(); } void Controller::updateFromXMLNode(SimTK::Xml::Element& node, @@ -169,8 +169,7 @@ void Controller::extendConnectToModel(Model& model) _actuatorSet.adoptAndAppend(&actuator); } return; - } - else{ + } else { for (int i = 0; i < nac; ++i) { bool found = false; for (auto& actuator : actuators) { @@ -200,15 +199,14 @@ void Controller::extendAddToSystem(SimTK::MultibodySystem& system) const // makes a request for which actuators a controller will control void Controller::setActuators(const Set& actuators) { - //TODO this needs to be setting a Socket list of Actuators - - // make sure controller does NOT assume ownership - _actuatorSet.setSize(0); - _actuatorSet.setMemoryOwner(false); - //Rebuild consistent set of actuator lists + // Rebuild a consistent set of actuator lists. updProperty_actuator_list().clear(); - for (int i = 0; i< actuators.getSize(); i++){ - addActuator(actuators[i]); + updProperty_actuators().clear(); + for (int i = 0; i < actuators.getSize(); ++i){ + append_actuators(ControllerActuator()); + auto& actu = upd_actuators(getProperty_actuators().size() - 1); + actu.connectSocket_actuator(actuators[i]); + append_actuator_list(actuators[i].getName()); } } @@ -222,6 +220,17 @@ void Controller::addActuator(const Actuator& actuator) updProperty_actuator_list().appendValue(actuator.getName()); } -Set& Controller::updActuators() { return _actuatorSet; } +Set& Controller::updActuators() { + return _actuatorSet; +} -const Set& Controller::getActuatorSet() const { return _actuatorSet; } +const Set& Controller::getActuatorSet() const { + Set actuatorSet; + + for (int i = 0; i < getProperty_actuators().size(); ++i) { + const auto& actuator = + get_actuators(i).getSocket("actuator").getConnecteeAsObject(); + } + + +} diff --git a/OpenSim/Simulation/Control/Controller.h b/OpenSim/Simulation/Control/Controller.h index 1c1d277e50..7347cacf08 100644 --- a/OpenSim/Simulation/Control/Controller.h +++ b/OpenSim/Simulation/Control/Controller.h @@ -37,6 +37,16 @@ namespace OpenSim { class Model; class Actuator; +class OSIMSIMULATION_API ControllerActuator : public Component { +OpenSim_DECLARE_CONCRETE_OBJECT(ControllerActuator, Component); + +public: + OpenSim_DECLARE_SOCKET(actuator, Actuator, + "The actuator that this controller controls."); + + ControllerActuator() = default; +}; + /** * Controller is an abstract ModelComponent that defines the interface for * an OpenSim Controller. A controller computes and sets the values of the @@ -55,9 +65,9 @@ class OSIMSIMULATION_API Controller : public ModelComponent { OpenSim_DECLARE_ABSTRACT_OBJECT(Controller, ModelComponent); public: -//============================================================================== +//============================================================================= // PROPERTIES -//============================================================================== +//============================================================================= /** Controller is enabled (active) by default. NOTE: Prior to OpenSim 4.0, this property was named **isDisabled**. If **isDisabled** is **true**, **enabled** is **false**. @@ -69,7 +79,10 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Controller, ModelComponent); OpenSim_DECLARE_LIST_PROPERTY(actuator_list, std::string, "The names of the model actuators that this controller will control." "The keyword ALL indicates the controller will control all the " - "actuators in the model" ); + "actuators in the model"); + + OpenSim_DECLARE_LIST_PROPERTY(actuators, ControllerActuator, + "The actuators controlled by this controller."); //============================================================================= // METHODS @@ -143,9 +156,6 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Controller, ModelComponent); // number of controls this controller computes int _numControls; - // the (sub)set of Model actuators that this controller controls */ - Set _actuatorSet; - // construct and initialize properties void constructProperties(); diff --git a/OpenSim/Simulation/Control/InputController.cpp b/OpenSim/Simulation/Control/InputController.cpp index 2a3cf00497..5e00a48597 100644 --- a/OpenSim/Simulation/Control/InputController.cpp +++ b/OpenSim/Simulation/Control/InputController.cpp @@ -41,8 +41,14 @@ InputController::~InputController() = default; InputController::InputController(const InputController& other) : Controller(other) {} -InputController& InputController::operator=(const InputController& other) - : Controller(other) { +InputController& InputController::operator=(const InputController& other) { + // Controller uses a custom copy constructor, so we need to explicitly + // call the base class assignment operator. + if (this != &other) { + Super::operator=(other); + m_controlNames = other.m_controlNames; + m_controlIndexes = other.m_controlIndexes; + } return *this; } @@ -76,7 +82,7 @@ std::vector InputController::getInputChannelAliases() const { //============================================================================= // MODEL COMPONENT INTERFACE //============================================================================= -void InputController::extendConnectToModel(Model& model) override { +void InputController::extendConnectToModel(Model& model) { Super::extendConnectToModel(model); // Get a list of the control names and indexes for all actuators in the @@ -134,7 +140,14 @@ ActuatorInputController::ActuatorInputController( const ActuatorInputController& other) : InputController(other) {} ActuatorInputController& ActuatorInputController::operator=( - const ActuatorInputController& other) : InputController(other) { + const ActuatorInputController& other) { + // Controller uses a custom copy constructor, so we need to explicitly + // call the base class assignment operator. + if (this != &other) { + Super::operator=(other); + m_controlIndexesInConnecteeOrder = + other.m_controlIndexesInConnecteeOrder; + } return *this; } @@ -148,7 +161,7 @@ ActuatorInputController& ActuatorInputController::operator=( // CONTROLLER INTERFACE //============================================================================= void ActuatorInputController::computeControls(const SimTK::State& s, - SimTK::Vector& controls) const override { + SimTK::Vector& controls) const { const auto& input = getInput("inputs"); for (int i = 0; i < input.getNumConnectees(); ++i) { controls[m_controlIndexesInConnecteeOrder[i]] = input.getValue(s, i); @@ -158,7 +171,7 @@ void ActuatorInputController::computeControls(const SimTK::State& s, //============================================================================= // MODEL COMPONENT INTERFACE //============================================================================= -void ActuatorInputController::extendConnectToModel(Model& model) override { +void ActuatorInputController::extendConnectToModel(Model& model) { Super::extendConnectToModel(model); const auto& controlNames = getControlNames(); @@ -187,4 +200,4 @@ void ActuatorInputController::extendConnectToModel(Model& model) override { m_controlIndexesInConnecteeOrder.push_back( controlIndexes[it - controlNames.begin()]); } -} \ No newline at end of file +} From e9a8b145099f42debf6f3c44085786de82c07d83 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Thu, 4 Jan 2024 16:11:01 -0800 Subject: [PATCH 14/48] Start converting Controller to use a list Socket --- OpenSim/Common/ComponentSocket.h | 14 +- .../Control/ControlSetController.cpp | 45 +++-- OpenSim/Simulation/Control/Controller.cpp | 187 +++++------------- OpenSim/Simulation/Control/Controller.h | 74 ++++--- .../Control/PrescribedController.cpp | 37 ++-- 5 files changed, 152 insertions(+), 205 deletions(-) diff --git a/OpenSim/Common/ComponentSocket.h b/OpenSim/Common/ComponentSocket.h index 0f07eff334..a5b28de0d5 100644 --- a/OpenSim/Common/ComponentSocket.h +++ b/OpenSim/Common/ComponentSocket.h @@ -253,6 +253,18 @@ class OSIMCOMMON_API AbstractSocket { return getConnecteePathProp().getValue().empty(); } + /** Check if this socket has a connectee path that matches the provided + * path. */ + bool hasConnecteePath(const std::string& path) const { + return getConnecteePathProp().findIndex(path) != -1; + } + + /** Get the index of the provided connectee path in the connectee path + * property. */ + int getConnecteePathIndex(const std::string& path) const { + return getConnecteePathProp().findIndex(path); + } + /** Get owner component of this socket */ const Component& getOwner() const { return _owner.getRef(); } @@ -426,7 +438,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; diff --git a/OpenSim/Simulation/Control/ControlSetController.cpp b/OpenSim/Simulation/Control/ControlSetController.cpp index 553aeecc23..8c5d4d7308 100644 --- a/OpenSim/Simulation/Control/ControlSetController.cpp +++ b/OpenSim/Simulation/Control/ControlSetController.cpp @@ -29,13 +29,15 @@ //============================================================================= // INCLUDES //============================================================================= -#include "Controller.h" #include "ControlSetController.h" + #include "ControlLinear.h" #include "ControlSet.h" -#include -#include +#include "Controller.h" +#include +#include +#include //============================================================================= // STATICS @@ -162,13 +164,14 @@ void ControlSetController::computeControls(const SimTK::State& s, SimTK::Vector& { SimTK_ASSERT( _controlSet , "ControlSetController::computeControls controlSet is NULL"); - std::string actName = ""; + std::string actName; int index = -1; - int na = getActuatorSet().getSize(); - - for(int i=0; i< na; ++i){ - actName = getActuatorSet()[i].getName(); + const auto& socket = getSocket("actuators"); + const int na = static_cast(socket.getNumConnectees()); + for(int i = 0; i < na; ++i){ + const auto& actu = socket.getConnectee(i); + actName = actu.getName(); index = _controlSet->getIndex(actName); if(index < 0){ actName = actName + ".excitation"; @@ -177,7 +180,7 @@ void ControlSetController::computeControls(const SimTK::State& s, SimTK::Vector& if(index >= 0){ SimTK::Vector actControls(1, _controlSet->get(index).getControlValue(s.getTime())); - getActuatorSet()[i].addInControls(actControls, controls); + actu.addInControls(actControls, controls); } } } @@ -261,6 +264,7 @@ void ControlSetController::extendFinalizeFromProperties() setEnabled(true); } + const auto& socket = updSocket("actuators"); std::string ext = ".excitation"; for (int i = 0; _controlSet != nullptr && i < _controlSet->getSize(); ++i) { std::string actName = _controlSet->get(i).getName(); @@ -268,7 +272,26 @@ void ControlSetController::extendFinalizeFromProperties() !(actName.compare(actName.length() - ext.length(), ext.length(), ext))) { actName.erase(actName.length() - ext.length(), ext.length()); } - if (getProperty_actuator_list().findIndex(actName) < 0) // not already in the list of actuators for this controller - updProperty_actuator_list().appendValue(actName); + + // Check if the actuator is already connected to this controller. + const auto& modelActuators = getModel().getComponentList(); + bool foundActuator = false; + for (const auto& actu : modelActuators) { + if (actu.getName() == actName) { + // Do not add the actuator if it is already in the list of + // connectee paths. The connectee paths should be valid at this + // point during deserialization. + if (!socket.hasConnecteePath(actu.getAbsolutePathString())) { + addActuator(actu); + } + foundActuator = true; + break; + } + } + + OPENSIM_THROW_IF_FRMOBJ(!foundActuator, Exception, + "ControlSetController::extendFinalizeFromProperties: " + "Actuator '{}' in ControlSet '{}' not found in model '{}'.", + actName, _controlSet->getName(), getModel().getName()); } } diff --git a/OpenSim/Simulation/Control/Controller.cpp b/OpenSim/Simulation/Control/Controller.cpp index 72a27b1741..0a9b069d74 100644 --- a/OpenSim/Simulation/Control/Controller.cpp +++ b/OpenSim/Simulation/Control/Controller.cpp @@ -27,78 +27,39 @@ #include "Controller.h" #include #include -#include //============================================================================= // STATICS //============================================================================= - using namespace OpenSim; using namespace std; - //============================================================================= // CONSTRUCTOR(S) AND DESTRUCTOR //============================================================================= -//_____________________________________________________________________________ -/** - * Default constructor. - */ Controller::Controller() : - ModelComponent{}, - _numControls{0}, - _actuatorSet{} -{ + ModelComponent{}, _numControls{0} { constructProperties(); } -Controller::Controller(Controller const& src) : - ModelComponent{src}, - PropertyIndex_enabled{src.PropertyIndex_enabled}, - PropertyIndex_actuator_list{src.PropertyIndex_actuator_list}, - _numControls{src._numControls}, - _actuatorSet{} -{ - // care: the reason this custom copy constructor exists is to prevent - // a memory leak (#3247) - _actuatorSet.setMemoryOwner(false); -} +Controller::~Controller() noexcept = default; -Controller& Controller::operator=(Controller const& src) -{ - // care: the reason this custom copy assignment exists is to prevent - // a memory leak (#3247) - - if (&src != this) - { - static_cast(*this) = static_cast(src); - PropertyIndex_enabled = src.PropertyIndex_enabled; - PropertyIndex_actuator_list = src.PropertyIndex_actuator_list; - _numControls = src._numControls; - _actuatorSet.setSize(0); - } - return *this; -} +Controller::Controller(const Controller&) = default; -Controller::~Controller() noexcept = default; +Controller::Controller(Controller&&) = default; -//============================================================================= -// CONSTRUCTION -//============================================================================= -//_____________________________________________________________________________ -//_____________________________________________________________________________ - -/** - * Connect properties to local pointers. - */ -void Controller::constructProperties() -{ +Controller& Controller::operator=(const Controller&) = default; + +Controller& Controller::operator=(Controller&&) = default; + +void Controller::constructProperties() { setAuthors("Ajay Seth, Frank Anderson, Chand John, Samuel Hamner"); constructProperty_enabled(true); - constructProperty_actuator_list(); - _actuatorSet.setMemoryOwner(false); } +//============================================================================= +// COMPONENT INTERFACE +//============================================================================= void Controller::updateFromXMLNode(SimTK::Xml::Element& node, int versionNumber) { if(versionNumber < XMLDocument::getLatestVersion()) { @@ -123,105 +84,57 @@ void Controller::updateFromXMLNode(SimTK::Xml::Element& node, Super::updateFromXMLNode(node, versionNumber); } +void Controller::extendConnectToModel(Model& model) { + Super::extendConnectToModel(model); + // TODO check if all actuators in connectee paths are in the model? +} + +void Controller::extendAddToSystem(SimTK::MultibodySystem& system) const { + Super::extendAddToSystem(system); +} + //============================================================================= // GET AND SET //============================================================================= - -//----------------------------------------------------------------------------- -// ON/OFF -//----------------------------------------------------------------------------- -//_____________________________________________________________________________ -/** - * Get whether or not this controller is enabled. - */ -bool Controller::isEnabled() const -{ +bool Controller::isEnabled() const { return get_enabled(); } -//_____________________________________________________________________________ -/** - * Turn this controller on or off. - */ -void Controller::setEnabled(bool aTrueFalse) -{ + +void Controller::setEnabled(bool aTrueFalse) { upd_enabled() = aTrueFalse; } -// for any post XML deserialization initialization -void Controller::extendConnectToModel(Model& model) -{ - Super::extendConnectToModel(model); - - // TODO this custom connection code can all disappear - // if we use a list Socket - - // make sure controller does not take ownership - _actuatorSet.setSize(0); - _actuatorSet.setMemoryOwner(false); - - int nac = getProperty_actuator_list().size(); - if (nac == 0) - return; - - auto actuators = model.getComponentList(); - if (IO::Uppercase(get_actuator_list(0)) == "ALL"){ - for (auto& actuator : actuators) { - _actuatorSet.adoptAndAppend(&actuator); - } - return; - } - else{ - for (int i = 0; i < nac; ++i) { - bool found = false; - for (auto& actuator : actuators) { - if (get_actuator_list(i) == actuator.getName()) { - _actuatorSet.adoptAndAppend(&actuator); - found = true; - break; - } - } - if (!found) { - cerr << "WARN: Controller::connectToModel : Actuator " - << get_actuator_list(i) << - " was not found and will be ignored." << endl; - } - } +void Controller::setActuators(const Set& actuators) { + updSocket("actuators").disconnect(); + for (int i = 0; i < actuators.getSize(); i++){ + const auto& actu = actuators.get(i); + addActuator(actuators[i]); } } -/** - * Create a Controller in the SimTK::System - */ -void Controller::extendAddToSystem(SimTK::MultibodySystem& system) const -{ - Super::extendAddToSystem(system); -} - -// makes a request for which actuators a controller will control -void Controller::setActuators(const Set& actuators) -{ - //TODO this needs to be setting a Socket list of Actuators - - // make sure controller does NOT assume ownership - _actuatorSet.setSize(0); - _actuatorSet.setMemoryOwner(false); - //Rebuild consistent set of actuator lists - updProperty_actuator_list().clear(); - for (int i = 0; i< actuators.getSize(); i++){ - addActuator(actuators[i]); +void Controller::setActuators(const SimTK::Array_& actuators) { + updSocket("actuators").disconnect(); + for (const auto& actu : actuators) { + addActuator(actu); } } - -void Controller::addActuator(const Actuator& actuator) -{ - _actuatorSet.adoptAndAppend(&actuator); - - int found = updProperty_actuator_list().findIndex(actuator.getName()); - if (found < 0) //add if the actuator isn't already in the list - updProperty_actuator_list().appendValue(actuator.getName()); +void Controller::addActuator(const Actuator& actuator) { + connectSocket_actuators(actuator); } -Set& Controller::updActuators() { return _actuatorSet; } - -const Set& Controller::getActuatorSet() const { return _actuatorSet; } +// const SimTK::Array_& Controller::getActuators() const { +// const auto& socket = getSocket("actuators"); +// const int nc = static_cast(socket.getNumConnectees()); +// for (int i = 0; i < nc; ++i) { +// const auto& actuator = socket.getConnectee(i); +// _actuators.push_back(&actuator); +// } +// +// } + + +// const Set& Controller::getActuatorSet() const { +// +// // TODO +// } diff --git a/OpenSim/Simulation/Control/Controller.h b/OpenSim/Simulation/Control/Controller.h index 1c1d277e50..7b7042b3dc 100644 --- a/OpenSim/Simulation/Control/Controller.h +++ b/OpenSim/Simulation/Control/Controller.h @@ -23,17 +23,11 @@ * limitations under the License. * * -------------------------------------------------------------------------- */ -//============================================================================ -// INCLUDE -//============================================================================ -// These files contain declarations and definitions of variables and methods -// that will be used by the Controller class. #include #include namespace OpenSim { -// Forward declarations of classes that are used by the controller implementation class Model; class Actuator; @@ -66,10 +60,8 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Controller, ModelComponent); "Flag (true or false) indicating whether or not the controller is " "enabled." ); - OpenSim_DECLARE_LIST_PROPERTY(actuator_list, std::string, - "The names of the model actuators that this controller will control." - "The keyword ALL indicates the controller will control all the " - "actuators in the model" ); + OpenSim_DECLARE_LIST_SOCKET(actuators, Actuator, + "The list of Actuators that this controller will control."); //============================================================================= // METHODS @@ -77,19 +69,17 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Controller, ModelComponent); //-------------------------------------------------------------------------- // CONSTRUCTION AND DESTRUCTION //-------------------------------------------------------------------------- -public: - - /** Default constructor. */ Controller(); - Controller(Controller const&); - Controller& operator=(Controller const&); ~Controller() noexcept override; - // Uses default (compiler-generated) destructor, copy constructor and copy - // assignment operator. + Controller(const Controller&); + Controller& operator=(Controller const&); + + Controller(Controller&&); + Controller& operator=(Controller&&); //-------------------------------------------------------------------------- - // Controller Interface + // CONTROLLER INTERFACE //-------------------------------------------------------------------------- /** Get whether or not this controller is enabled. * @return true when controller is enabled. @@ -101,14 +91,20 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Controller, ModelComponent); */ void setEnabled(bool enableFlag); - /** replace the current set of actuators with the provided set */ - void setActuators(const Set& actuators ); - /** add to the current set of actuators */ + /** Replace the current set of actuators with the provided set. */ + void setActuators(const Set& actuators); + void setActuators(const SimTK::Array_& actuators); + + /** Add to the current set of actuators. */ void addActuator(const Actuator& actuator); - /** get a const reference to the current set of const actuators */ - const Set& getActuatorSet() const; - /** get a writable reference to the set of const actuators for this controller */ - Set& updActuators(); + + /** Get a const reference to the current set of const actuators. */ + // const Set& getActuatorSet() const; + //const SimTK::Array_& getActuators() const; + + /** Get a writable reference to the set of const actuators for this + * controller. */ + // Set& updActuators(); /** Compute the control for actuator * This method defines the behavior for any concrete controller @@ -120,41 +116,37 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Controller, ModelComponent); virtual void computeControls(const SimTK::State& s, SimTK::Vector &controls) const = 0; - int getNumControls() const {return _numControls;} + /** Get the number of controls this controller computes. */ + int getNumControls() const { return _numControls; } protected: /** Model component interface that permits the controller to be "wired" up - to its actuators. Subclasses can override to perform additional setup. */ + * to its actuators. Subclasses can override to perform additional setup. */ void extendConnectToModel(Model& model) override; - /** Model component interface that creates underlying computational components - in the SimTK::MultibodySystem. This includes adding states, creating - measures, etc... required by the controller. */ + /** Model component interface that creates underlying computational + * components in the SimTK::MultibodySystem. This includes adding states, + * creating measures, etc., required by the controller. */ void extendAddToSystem(SimTK::MultibodySystem& system) const override; - /** Only a Controller can set its number of controls based on its actuators */ + /** Only a Controller can set its number of controls based on its + * actuators. */ void setNumControls(int numControls) {_numControls = numControls; } void updateFromXMLNode(SimTK::Xml::Element& node, int versionNumber) override; private: - // number of controls this controller computes + // The number of controls this controller computes. int _numControls; - // the (sub)set of Model actuators that this controller controls */ - Set _actuatorSet; - - // construct and initialize properties + // Construct and initialize properties. void constructProperties(); -//============================================================================= -}; // END of class Controller +}; // class Controller -}; //namespace -//============================================================================= -//============================================================================= +} // namespace OpenSim #endif // OPENSIM_CONTROLLER_H_ diff --git a/OpenSim/Simulation/Control/PrescribedController.cpp b/OpenSim/Simulation/Control/PrescribedController.cpp index a699df7a0f..aa9bf2e9ae 100644 --- a/OpenSim/Simulation/Control/PrescribedController.cpp +++ b/OpenSim/Simulation/Control/PrescribedController.cpp @@ -1,4 +1,4 @@ -/* -------------------------------------------------------------------------- * + /* -------------------------------------------------------------------------- * * OpenSim: PrescribedController.cpp * * -------------------------------------------------------------------------- * * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * @@ -122,7 +122,8 @@ void PrescribedController::extendConnectToModel(Model& model) FunctionSet& controlFuncs = upd_ControlFunctions(); const Set& modelActuators = getModel().getActuators(); - Set& controllerActuators = updActuators(); + //Set& controllerActuators = updActuators(); + const auto& socket = getSocket("actuators"); for(int i=0; igetName()); + Function* pfunc = + createFunctionFromData(columnLabel, time, data); + // If not already assigned to this controller, assign it. + //int inC = controllerActuators.getIndex(actuator->getName()); + // TODO do I need to check if the actuator is actually connected + // at this point? + int inC = socket.getConnecteePathIndex( + actuator->getAbsolutePathString()); if(inC >= 0) prescribeControlForActuator(inC, pfunc); else{ // add the actuator to the controller's list - updProperty_actuator_list().appendValue(actuator->getName()); - controllerActuators.adoptAndAppend(actuator); + addActuator(*actuator); prescribeControlForActuator(actuator->getName(), pfunc); } }// if found in functions, it has already been prescribed @@ -167,10 +172,11 @@ void PrescribedController::computeControls(const SimTK::State& s, SimTK::Vector& SimTK::Vector actControls(1, 0.0); SimTK::Vector time(1, s.getTime()); - for(int i=0; i("actuators"); + for(int i = 0; i < socket.getNumConnectees(); ++i){ actControls[0] = get_ControlFunctions()[i].calcValue(time); - getActuatorSet()[i].addInControls(actControls, controls); - } + socket.getConnectee(i).addInControls(actControls, controls); + } } @@ -184,18 +190,19 @@ void PrescribedController:: OPENSIM_THROW_IF_FRMOBJ(index < 0, Exception, "Index was " + std::to_string(index) + " but must be nonnegative." ); - OPENSIM_THROW_IF(index >= getActuatorSet().getSize(), + + int numActuators = getSocket("actuators").getNumConnectees(); + OPENSIM_THROW_IF(index >= numActuators, IndexOutOfRange, (size_t)index, 0, - (size_t)getActuatorSet().getSize() - 1); + (size_t)numActuators - 1); if(index >= get_ControlFunctions().getSize()) upd_ControlFunctions().setSize(index+1); upd_ControlFunctions().set(index, prescribedFunction); } -void PrescribedController:: - prescribeControlForActuator(const std::string actName, - Function *prescribedFunction) +void PrescribedController::prescribeControlForActuator( + const std::string actName, Function *prescribedFunction) { int index = getProperty_actuator_list().findIndex(actName); if(index < 0 ) From 2c305f4b7707f774eda1be836cf326195e5de3f6 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 8 Jan 2024 10:07:53 -0800 Subject: [PATCH 15/48] Convert to a list Socket for Controller actuators --- OpenSim/CMakeLists.txt | 2 +- OpenSim/Common/ComponentSocket.h | 4 +- OpenSim/Sandbox/Moco/sandboxSandbox.cpp | 64 ++++++++++++++++--- OpenSim/Simulation/Control/Controller.h | 5 ++ .../Control/PrescribedController.cpp | 10 ++- OpenSim/Simulation/Model/ControllerSet.cpp | 9 +-- OpenSim/Tools/ActuatorForceTarget.cpp | 17 +++-- OpenSim/Tools/ActuatorForceTargetFast.cpp | 52 ++++++++------- OpenSim/Tools/CMC.cpp | 18 +++--- OpenSim/Tools/CMCTool.cpp | 2 +- OpenSim/Tools/CorrectionController.cpp | 5 +- OpenSim/Tools/VectorFunctionForActuators.cpp | 8 ++- 12 files changed, 126 insertions(+), 70 deletions(-) diff --git a/OpenSim/CMakeLists.txt b/OpenSim/CMakeLists.txt index 8d53dedd8f..8ba2fcc16e 100644 --- a/OpenSim/CMakeLists.txt +++ b/OpenSim/CMakeLists.txt @@ -10,6 +10,6 @@ add_subdirectory(Moco) add_subdirectory(Examples) add_subdirectory(Tests) -#add_subdirectory(Sandbox) +add_subdirectory(Sandbox) install(FILES OpenSim.h DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/OpenSim") diff --git a/OpenSim/Common/ComponentSocket.h b/OpenSim/Common/ComponentSocket.h index a5b28de0d5..4bd99471a7 100644 --- a/OpenSim/Common/ComponentSocket.h +++ b/OpenSim/Common/ComponentSocket.h @@ -398,7 +398,7 @@ class Socket : public AbstractSocket { } /** Return a const reference to the Object connected to this Socket. */ - const T& getConnecteeAsObject(int index = -1) const override { + const Object& getConnecteeAsObject(int index = -1) const override { if (index < 0) { if (!isListSocket()) { index = 0; } else { @@ -483,7 +483,7 @@ class Socket : public AbstractSocket { private: - const T& getConnecteeAsObjectInternal(int index) const { + const Object& getConnecteeAsObjectInternal(int index) const { OPENSIM_THROW_IF(!isConnected(), Exception, "Socket '{}' not connected.", getName()); using SimTK::isIndexInRange; diff --git a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp index d935990b59..265286ca33 100644 --- a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp +++ b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp @@ -19,19 +19,63 @@ // This file provides a way to easily prototype or test temporary snippets of // code during development. -#include +#include +#include using namespace OpenSim; +class TestController : public Controller { +OpenSim_DECLARE_CONCRETE_OBJECT(TestController, Controller); +public: + void computeControls(const SimTK::State& s, + SimTK::Vector& controls) const override { + log_cout("Computing controls..."); + } + + void setActuatorPath(const std::string& path) { + m_actuatorPath = path; + } + + const std::string& getActuatorPath() const { + return m_actuatorPath; + } + +protected: + void extendConnectToModel(Model& model) override { + const auto& socket = getSocket("actuators"); + const auto& actuPath = getActuatorPath(); + int index = socket.getConnecteePathIndex(actuPath); + log_cout("extendConnectToModel: connectee path index = {}", index); + + } + + void extendAddToSystem(SimTK::MultibodySystem& system) const override { + const auto& socket = getSocket("actuators"); + const auto& actuPath = getActuatorPath(); + int index = socket.getConnecteePathIndex(actuPath); + log_cout("extendAddToSystem: connectee path index = {}", index); + } + +private: + std::string m_actuatorPath; + +}; + int main() { - // TODO Logger::setLevel(Logger::Level::Debug); - MocoTrack track; - ModelProcessor modelProcessor("DeMers_mod_noarms_welds_4.0.osim"); - modelProcessor.append(ModOpReplaceMusclesWithDeGrooteFregly2016()); - modelProcessor.append(ModOpIgnoreTendonCompliance()); - track.setModel(modelProcessor); - track.setStatesReference({"r_SLD_mean_coords.sto"}); - track.set_allow_unused_references(true); - MocoSolution solution = track.solve(); + + Model model = ModelFactory::createNLinkPendulum(5); + + auto* controller = new TestController(); + controller->setActuatorPath("/tau3"); + controller->connectSocket_actuators(model.getComponent("tau0")); + controller->connectSocket_actuators(model.getComponent("tau1")); + controller->connectSocket_actuators(model.getComponent("tau2")); + controller->connectSocket_actuators(model.getComponent("tau3")); + controller->connectSocket_actuators(model.getComponent("tau4")); + + model.addController(controller); + + model.initSystem(); + return EXIT_SUCCESS; } diff --git a/OpenSim/Simulation/Control/Controller.h b/OpenSim/Simulation/Control/Controller.h index 7b7042b3dc..1c07aac494 100644 --- a/OpenSim/Simulation/Control/Controller.h +++ b/OpenSim/Simulation/Control/Controller.h @@ -119,6 +119,11 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Controller, ModelComponent); /** Get the number of controls this controller computes. */ int getNumControls() const { return _numControls; } + int getNumActuators() const { + return static_cast( + getSocket("actuators").getNumConnectees()); + } + protected: /** Model component interface that permits the controller to be "wired" up diff --git a/OpenSim/Simulation/Control/PrescribedController.cpp b/OpenSim/Simulation/Control/PrescribedController.cpp index aa9bf2e9ae..8e9fb4efd7 100644 --- a/OpenSim/Simulation/Control/PrescribedController.cpp +++ b/OpenSim/Simulation/Control/PrescribedController.cpp @@ -204,9 +204,13 @@ void PrescribedController:: void PrescribedController::prescribeControlForActuator( const std::string actName, Function *prescribedFunction) { - int index = getProperty_actuator_list().findIndex(actName); - if(index < 0 ) - throw Exception("PrescribedController does not have "+actName+" in its list of actuators to control."); + // TODO how to get the actuator path from the name? + const auto& socket = getSocket("actuators"); + int index = socket.getConnecteePathIndex(actName); + OPENSIM_THROW_IF_FRMOBJ(index < 0, Exception, + "PrescribedController does not have {} in its list of actuators to " + "control.", actName) + prescribeControlForActuator(index, prescribedFunction); } diff --git a/OpenSim/Simulation/Model/ControllerSet.cpp b/OpenSim/Simulation/Model/ControllerSet.cpp index cb3eff9100..204a522d84 100644 --- a/OpenSim/Simulation/Model/ControllerSet.cpp +++ b/OpenSim/Simulation/Model/ControllerSet.cpp @@ -101,11 +101,12 @@ void ControllerSet::printInfo() const i+1, (unsigned long long)&c, c.getName().c_str(), (unsigned long long)&c.getModel() ); - const Set& actSet = c.getActuatorSet(); - if( actSet.getSize() > 0 ) { + const auto& socket = c.getSocket("actuators"); + int nc = socket.getNumConnectees(); + if(nc > 0) { log_cout("Actuators"); - for(int j=0;jgetModel().getNumSpeeds(); - int nf = _controller->getActuatorSet().getSize(); + int nf = _controller->getNumActuators(); // int ny = _controller->getModel().getNumStateVariables(); int nacc = _controller->updTaskSet().getDesiredAccelerations().getSize(); @@ -228,10 +228,9 @@ prepareToOptimize(SimTK::State& s, double *x) void ActuatorForceTarget:: computePerformanceVectors(SimTK::State& s, const Vector &aF, Vector &rAccelPerformanceVector, Vector &rForcePerformanceVector) { - const Set &fSet = _controller->getActuatorSet(); - - for(int i=0;i(&fSet[i]); + const auto& socket = _controller->getSocket("actuators"); + for(int i = 0; i < _controller->getNumActuators(); i++) { + auto act = dynamic_cast(&socket.getConnectee(i)); act->setOverrideActuation(s, aF[i]); act->overrideActuation(s, true); } @@ -246,8 +245,8 @@ computePerformanceVectors(SimTK::State& s, const Vector &aF, Vector &rAccelPerfo // PERFORMANCE double sqrtStressTermWeight = sqrt(_stressTermWeight); - for(int i=0;i(&fSet[i]); + for(int i = 0; i < _controller->getNumActuators(); i++) { + auto act = dynamic_cast(&socket.getConnectee(i)); rForcePerformanceVector[i] = sqrtStressTermWeight * act->getStress(s); } @@ -255,8 +254,8 @@ computePerformanceVectors(SimTK::State& s, const Vector &aF, Vector &rAccelPerfo for(int i=0;i(&fSet[i]); + for(int i = 0; i < _controller->getNumActuators(); i++) { + auto act = dynamic_cast(&socket.getConnectee(i)); act->overrideActuation(s, false); } } diff --git a/OpenSim/Tools/ActuatorForceTargetFast.cpp b/OpenSim/Tools/ActuatorForceTargetFast.cpp index 435576f37b..7d84426e7c 100644 --- a/OpenSim/Tools/ActuatorForceTargetFast.cpp +++ b/OpenSim/Tools/ActuatorForceTargetFast.cpp @@ -71,7 +71,7 @@ ActuatorForceTargetFast(SimTK::State& s, int aNX,CMC *aController): int ny = _controller->getModel().getNumStateVariables(); int nq = _controller->getModel().getNumCoordinates(); int nu = _controller->getModel().getNumSpeeds(); - int na = _controller->getActuatorSet().getSize(); + int na = _controller->getNumActuators(); _y.setSize(ny); _dydt.setSize(ny); @@ -93,9 +93,9 @@ ActuatorForceTargetFast(SimTK::State& s, int aNX,CMC *aController): // COMPUTE ACTUATOR AREAS Array f(1.0,na); - const Set& fSet = _controller->getActuatorSet(); - for(int i=0,j=0;i(&fSet[i]); + const auto& socket = _controller->getSocket("actuators"); + for(int i = 0, j = 0; i < na; i++) { + auto act = dynamic_cast(&socket.getConnectee(i)); auto musc = dynamic_cast(act); if(musc) _recipAreaSquared[j] = f[j]/musc->getMaxIsometricForce(); @@ -117,21 +117,22 @@ prepareToOptimize(SimTK::State& s, double *x) // Keep around a "copy" of the state so we can use it in objective function // in cases where we're tracking states _saveState = s; + int na = _controller->getNumActuators(); + #ifdef USE_LINEAR_CONSTRAINT_MATRIX - int nf = _controller->getActuatorSet().getSize(); int nc = getNumConstraints(); - _constraintMatrix.resize(nc,nf); + _constraintMatrix.resize(nc,na); _constraintVector.resize(nc); - Vector f(nf), c(nc); + Vector f(na), c(nc); // Build linear constraint matrix and constant constraint vector f = 0; computeConstraintVector(s, f, _constraintVector); - for(int j=0; jgetModel().getMultibodySystem().realize( tempState, SimTK::Stage::Dynamics ); // COMPUTE MAX ISOMETRIC FORCE - const Set& fSet = _controller->getActuatorSet(); - double fOpt = SimTK::NaN; - getController()->getModel().getMultibodySystem().realize(tempState, SimTK::Stage::Dynamics ); - for(int i=0 ; i(&fSet[i]); + const auto& socket = _controller->getSocket("actuators"); + for(int i = 0; i < na; ++i) { + auto act = dynamic_cast(&socket.getConnectee(i)); auto mus = dynamic_cast(act); if(mus==NULL) { fOpt = act->getOptimalForce(); @@ -194,11 +193,11 @@ prepareToOptimize(SimTK::State& s, double *x) int ActuatorForceTargetFast:: objectiveFunc(const Vector &aF, const bool new_coefficients, Real& rP) const { - const Set& fSet = _controller->getActuatorSet(); double p = 0.0; const CMC_TaskSet& tset=_controller->getTaskSet(); - for(int i=0,j=0;i(&fSet[i]); + const auto& socket = _controller->getSocket("actuators"); + for(int i = 0, j = 0; i < _controller->getNumActuators(); i++) { + auto act = dynamic_cast(&socket.getConnectee(i)); auto mus = dynamic_cast(act); if(mus) { p += aF[j] * aF[j] * _recipOptForceSquared[j]; @@ -235,10 +234,10 @@ objectiveFunc(const Vector &aF, const bool new_coefficients, Real& rP) const int ActuatorForceTargetFast:: gradientFunc(const Vector &x, const bool new_coefficients, Vector &gradient) const { - const Set& fSet = _controller->getActuatorSet(); // double p = 0.0; - for(int i=0,index=0;i(&fSet[i]); + const auto& socket = _controller->getSocket("actuators"); + for (int i = 0, index = 0; i < _controller->getNumActuators(); i++) { + auto act = dynamic_cast(&socket.getConnectee(i)); auto mus = dynamic_cast(act); if(mus) { gradient[index] = 2.0 * x[index] * _recipOptForceSquared[index]; @@ -296,16 +295,15 @@ constraintFunc(const SimTK::Vector &x, const bool new_coefficients, SimTK::Vecto void ActuatorForceTargetFast:: computeConstraintVector(SimTK::State& s, const Vector &x,Vector &c) const { - CMC_TaskSet& taskSet = _controller->updTaskSet(); - const Set& fSet = _controller->getActuatorSet(); - - int nf = fSet.getSize(); + CMC_TaskSet& taskSet = _controller->updTaskSet(); + int na = _controller->getNumActuators(); + const auto& socket = _controller->getSocket("actuators"); // Now override the actuator forces with computed active force // (from static optimization) but also include the passive force // contribution of muscles when applying forces to the model - for(int i=0;i(&fSet[i]); + for(int i = 0; i < na; i++) { + auto act = dynamic_cast(&socket.getConnectee(i)); act->overrideActuation(s, true); act->setOverrideActuation(s, x[i]); } @@ -321,8 +319,8 @@ computeConstraintVector(SimTK::State& s, const Vector &x,Vector &c) const c[i]=w[i]*(aDes[i]-a[i]); // reset the actuator control - for(int i=0;i(&fSet[i]); + for(int i = 0; i < na; i++) { + auto act = dynamic_cast(&socket.getConnectee(i)); act->overrideActuation(s, false); } diff --git a/OpenSim/Tools/CMC.cpp b/OpenSim/Tools/CMC.cpp index 006efe8a57..f7c31fa0af 100644 --- a/OpenSim/Tools/CMC.cpp +++ b/OpenSim/Tools/CMC.cpp @@ -869,8 +869,9 @@ computeControls(SimTK::State& s, ControlSet &controlSet) for(i=0;i("actuators").getConnectee(i); log_warn("CMC::computeControls: small force range for {} ({} to {})", - getActuatorSet()[i].getName(), fmin[i], fmax[i]); + actu.getName(), fmin[i], fmax[i]); log_info(""); // if the force range is so small it means the control value, x, @@ -1098,9 +1099,10 @@ void CMC::computeControls(const SimTK::State& s, SimTK::Vector& controls) const "CMC::computeControls number of controls does not match number of actuators."); SimTK::Vector actControls(1, 0.0); - for(int i=0; i("actuators"); + for(int i = 0; i < socket.getNumConnectees(); i++){ actControls[0] = _controlSet[_controlSetIndices[i]].getControlValue(s.getTime()); - getActuatorSet()[i].addInControls(actControls, controls); + socket.getConnectee(i).addInControls(actControls, controls); } // double *val = &controls[0]; @@ -1138,10 +1140,8 @@ void CMC::extendAddToSystem( SimTK::MultibodySystem& system) const system.updDefaultSubsystem().addEventHandler(computeControlsHandler ); - const Set& fSet = getActuatorSet(); - int nActs = fSet.getSize(); - mutableThis->_controlSetIndices.setSize(nActs); + mutableThis->_controlSetIndices.setSize(getNumActuators()); // Create the control set that will hold the controls computed by CMC mutableThis->_controlSet.setName(_model->getName()); @@ -1152,10 +1152,10 @@ void CMC::extendAddToSystem( SimTK::MultibodySystem& system) const double xmin =0, xmax=0; std::string actName = ""; - - for(int i=0; i < nActs; ++i ) { + const auto& socket = getSocket("actuators"); + for(int i=0; i < getNumActuators(); ++i ) { - auto* act = dynamic_cast(&fSet[i]); + auto* act = dynamic_cast(&socket.getConnectee(i)); //Actuator& act = getActuatorSet().get(i); ControlLinear *control = new ControlLinear(); diff --git a/OpenSim/Tools/CMCTool.cpp b/OpenSim/Tools/CMCTool.cpp index 547f3b93b0..41a47a4216 100644 --- a/OpenSim/Tools/CMCTool.cpp +++ b/OpenSim/Tools/CMCTool.cpp @@ -541,7 +541,7 @@ bool CMCTool::run() int nq = _model->getNumCoordinates(); int nu = _model->getNumSpeeds(); - int na = controller->getActuatorSet().getSize(); + int na = controller->getNumActuators(); // Form complete storage objects for the q's and u's // This means filling in unspecified generalized coordinates and diff --git a/OpenSim/Tools/CorrectionController.cpp b/OpenSim/Tools/CorrectionController.cpp index 88002d1f53..515ff79349 100644 --- a/OpenSim/Tools/CorrectionController.cpp +++ b/OpenSim/Tools/CorrectionController.cpp @@ -291,7 +291,10 @@ void CorrectionController::extendConnectToModel(Model& model) addActuator(*actuator); } - setNumControls(getActuatorSet().getSize()); + // We're only using CoordinateActuators here, so the number of actuators + // should always match the number of controls. + // TODO is the number of connectees correct at this point? + setNumControls(getNumActuators()); log_info("CorrectionController::extendConnectToModel(): " "numActuators = {:d}, kv = {:0.3f}, kp = {:0.3f}", diff --git a/OpenSim/Tools/VectorFunctionForActuators.cpp b/OpenSim/Tools/VectorFunctionForActuators.cpp index 894acf00ce..090267dcc6 100644 --- a/OpenSim/Tools/VectorFunctionForActuators.cpp +++ b/OpenSim/Tools/VectorFunctionForActuators.cpp @@ -50,7 +50,7 @@ VectorFunctionForActuators::~VectorFunctionForActuators() */ VectorFunctionForActuators:: VectorFunctionForActuators(SimTK::System *aActuatorSystem, Model* model, CMCActuatorSubsystem* actSubsystem) : - VectorFunctionUncoupledNxN(model->getControllerSet().get("CMC" ).getActuatorSet().getSize() ), + VectorFunctionUncoupledNxN(model->getControllerSet().get("CMC" ).getNumActuators() ), _f(0.0) { setNull(); @@ -266,11 +266,13 @@ evaluate(const SimTK::State& s, const double *aX, double *rF) ts.initialize(actSysState); ts.stepTo(_tf); - const Set& forceSet = controller.getActuatorSet(); + // TODO check that the socket actuators are in the correct order for the + // target forces + const auto& socket = controller.getSocket("actuators"); // Vector function values int j = 0; for(i=0;i(&forceSet[i]); + auto act = dynamic_cast(&socket.getConnectee(i)); rF[j] = act->getActuation(getCMCActSubsys()->getCompleteState()) - _f[j]; j++; } From 7e290ff5cf8dd49bf7d71440def05b58f7d43b30 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Tue, 9 Jan 2024 13:46:41 -0800 Subject: [PATCH 16/48] Last update after Controller changes needed for compilation --- .../ExampleComponents/ToyReflexController.cpp | 35 +++++++---------- .../ControllerExample/ControllerExample.cpp | 5 ++- OpenSim/Simulation/Control/Controller.cpp | 38 +++++++++---------- OpenSim/Simulation/Control/Controller.h | 15 +++----- .../Tests/AddComponents/testAddComponents.cpp | 4 +- OpenSim/Tests/Wrapping/testWrapping.cpp | 3 +- 6 files changed, 45 insertions(+), 55 deletions(-) diff --git a/OpenSim/ExampleComponents/ToyReflexController.cpp b/OpenSim/ExampleComponents/ToyReflexController.cpp index ba03186819..14fd4a7c4b 100644 --- a/OpenSim/ExampleComponents/ToyReflexController.cpp +++ b/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& actuators = updActuators(); - - int cnt=0; - - while(cnt < actuators.getSize()){ - const Muscle *musc = dynamic_cast(&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("actuators"); + for (int i = 0; i < socket.getNumConnectees(); ++i) { + const auto& actu = socket.getConnectee(i); + const auto* musc = dynamic_cast(&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& actuators = getActuatorSet(); + // Get the Socket to list of actuators assigned to the reflex controller. + const auto& socket = getSocket("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[i]); + for (int i = 0; i < socket.getNumConnectees(); ++i) { + const auto& actu = socket.getConnectee(i); + const Muscle *musc = dynamic_cast(&actu); speed = musc->getLengtheningSpeed(s); // un-normalize muscle's maximum contraction velocity (fib_lengths/sec) max_speed = diff --git a/OpenSim/Examples/ControllerExample/ControllerExample.cpp b/OpenSim/Examples/ControllerExample/ControllerExample.cpp index 594e495076..86b67a6778 100644 --- a/OpenSim/Examples/ControllerExample/ControllerExample.cpp +++ b/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 ( &getActuatorSet().get(0) ); - auto rightMuscle = dynamic_cast ( &getActuatorSet().get(1) ); + const auto& socket = getSocket("actuators"); + auto leftMuscle = dynamic_cast(&socket.getConnectee(0)); + auto rightMuscle = dynamic_cast(&socket.getConnectee(1)); // Compute the desired position of the block in the tug-of-war // model. diff --git a/OpenSim/Simulation/Control/Controller.cpp b/OpenSim/Simulation/Control/Controller.cpp index 0a9b069d74..1239647655 100644 --- a/OpenSim/Simulation/Control/Controller.cpp +++ b/OpenSim/Simulation/Control/Controller.cpp @@ -107,34 +107,32 @@ void Controller::setEnabled(bool aTrueFalse) { void Controller::setActuators(const Set& actuators) { updSocket("actuators").disconnect(); for (int i = 0; i < actuators.getSize(); i++){ - const auto& actu = actuators.get(i); - addActuator(actuators[i]); + addActuator(actuators.get(i)); } } -void Controller::setActuators(const SimTK::Array_& actuators) { +void Controller::setActuators( + const SimTK::Array_>& actuators) { updSocket("actuators").disconnect(); for (const auto& actu : actuators) { - addActuator(actu); + addActuator(actu.getRef()); } } void Controller::addActuator(const Actuator& actuator) { - connectSocket_actuators(actuator); + appendConnectee_actuators(actuator); +} + +SimTK::Array_> +Controller::getActuators() const { + const auto& socket = getSocket("actuators"); + int nc = static_cast(socket.getNumConnectees()); + SimTK::Array_> actuators(nc); + for (int i = 0; i < socket.getNumConnectees(); ++i) { + const Actuator& actu = socket.getConnectee(i); + actuators[i] = &actu; + } + + return actuators; } -// const SimTK::Array_& Controller::getActuators() const { -// const auto& socket = getSocket("actuators"); -// const int nc = static_cast(socket.getNumConnectees()); -// for (int i = 0; i < nc; ++i) { -// const auto& actuator = socket.getConnectee(i); -// _actuators.push_back(&actuator); -// } -// -// } - - -// const Set& Controller::getActuatorSet() const { -// -// // TODO -// } diff --git a/OpenSim/Simulation/Control/Controller.h b/OpenSim/Simulation/Control/Controller.h index 1c07aac494..32f2caca46 100644 --- a/OpenSim/Simulation/Control/Controller.h +++ b/OpenSim/Simulation/Control/Controller.h @@ -93,18 +93,14 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Controller, ModelComponent); /** Replace the current set of actuators with the provided set. */ void setActuators(const Set& actuators); - void setActuators(const SimTK::Array_& actuators); + void setActuators( + const SimTK::Array_>& actuators); /** Add to the current set of actuators. */ void addActuator(const Actuator& actuator); - /** Get a const reference to the current set of const actuators. */ - // const Set& getActuatorSet() const; - //const SimTK::Array_& getActuators() const; - - /** Get a writable reference to the set of const actuators for this - * controller. */ - // Set& updActuators(); + /** Get an array of reference pointers to the current set of actuators. */ + SimTK::Array_> getActuators() const; /** Compute the control for actuator * This method defines the behavior for any concrete controller @@ -119,6 +115,7 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Controller, ModelComponent); /** Get the number of controls this controller computes. */ int getNumControls() const { return _numControls; } + /** Get the number of actuators that this controller is connected to. */ int getNumActuators() const { return static_cast( getSocket("actuators").getNumConnectees()); @@ -137,7 +134,7 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Controller, ModelComponent); /** Only a Controller can set its number of controls based on its * actuators. */ - void setNumControls(int numControls) {_numControls = numControls; } + void setNumControls(int numControls) { _numControls = numControls; } void updateFromXMLNode(SimTK::Xml::Element& node, int versionNumber) override; diff --git a/OpenSim/Tests/AddComponents/testAddComponents.cpp b/OpenSim/Tests/AddComponents/testAddComponents.cpp index 5f43a8bcb2..5f585906d4 100644 --- a/OpenSim/Tests/AddComponents/testAddComponents.cpp +++ b/OpenSim/Tests/AddComponents/testAddComponents.cpp @@ -262,8 +262,8 @@ void addComponentsToModel(Model& osimModel) // Create a prescribed controller that simply applies controls as function of time // For muscles, controls are normalized motor-neuron excitations PrescribedController *muscleController = new PrescribedController(); - muscleController->set_actuator_list(0, "muscle1"); - muscleController->set_actuator_list(1, "muscle2"); + muscleController->addActuator(*muscle1); + muscleController->addActuator(*muscle2); // Define linear functions for the control values for the two muscles Array slopeAndIntercept1(0.0, 2); // array of 2 doubles Array slopeAndIntercept2(0.0, 2); diff --git a/OpenSim/Tests/Wrapping/testWrapping.cpp b/OpenSim/Tests/Wrapping/testWrapping.cpp index 7064cc770b..3419a9d2b4 100644 --- a/OpenSim/Tests/Wrapping/testWrapping.cpp +++ b/OpenSim/Tests/Wrapping/testWrapping.cpp @@ -83,7 +83,8 @@ namespace { // force. PrescribedController actuController; actuController.setActuators(model.updActuators()); - for (int i = 0; i < actuController.getActuatorSet().getSize(); i++) { + const auto& socket = actuController.getSocket("actuators"); + for (int i = 0; i < socket.getNumConnectees(); i++) { actuController.prescribeControlForActuator( i, new Constant(activation)); } From fae7c57969c534185f82e8cbc5ec9dd1affb9bdb Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Thu, 11 Jan 2024 14:56:49 -0800 Subject: [PATCH 17/48] Various comments and formatting updates --- OpenSim/Common/ComponentSocket.h | 2 +- .../ExampleComponents/ToyReflexController.cpp | 2 +- OpenSim/Sandbox/Moco/sandboxSandbox.cpp | 19 +- .../Control/ControlSetController.cpp | 31 +-- .../Simulation/Control/ControlSetController.h | 5 +- OpenSim/Simulation/Control/Controller.h | 7 +- .../Control/PrescribedController.cpp | 245 ++++++++---------- .../Simulation/Control/PrescribedController.h | 62 +++-- OpenSim/Tools/Test/testControllers.cpp | 13 +- 9 files changed, 187 insertions(+), 199 deletions(-) diff --git a/OpenSim/Common/ComponentSocket.h b/OpenSim/Common/ComponentSocket.h index 6f37e3b3b1..c5e962848d 100644 --- a/OpenSim/Common/ComponentSocket.h +++ b/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. diff --git a/OpenSim/ExampleComponents/ToyReflexController.cpp b/OpenSim/ExampleComponents/ToyReflexController.cpp index 14fd4a7c4b..82a4659938 100644 --- a/OpenSim/ExampleComponents/ToyReflexController.cpp +++ b/OpenSim/ExampleComponents/ToyReflexController.cpp @@ -90,7 +90,7 @@ void ToyReflexController::extendConnectToModel(Model &model) void ToyReflexController::computeControls(const State& s, Vector &controls) const { - // Get the Socket to list of actuators assigned to the reflex controller. + // Get the Socket to the list of actuators assigned to the reflex controller. const auto& socket = getSocket("actuators"); // muscle lengthening speed diff --git a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp index 265286ca33..93491f8e0f 100644 --- a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp +++ b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp @@ -56,6 +56,15 @@ OpenSim_DECLARE_CONCRETE_OBJECT(TestController, Controller); log_cout("extendAddToSystem: connectee path index = {}", index); } + void extendFinalizeFromProperties() override { + + const auto& socket = getSocket("actuators"); + const auto& actuPath = getActuatorPath(); + int index = socket.getConnecteePathIndex(actuPath); + log_cout("extendAddToSystem: connectee path index = {}", index); + + } + private: std::string m_actuatorPath; @@ -67,11 +76,11 @@ int main() { auto* controller = new TestController(); controller->setActuatorPath("/tau3"); - controller->connectSocket_actuators(model.getComponent("tau0")); - controller->connectSocket_actuators(model.getComponent("tau1")); - controller->connectSocket_actuators(model.getComponent("tau2")); - controller->connectSocket_actuators(model.getComponent("tau3")); - controller->connectSocket_actuators(model.getComponent("tau4")); + controller->appendConnectee_actuators(model.getComponent("tau0")); + controller->appendConnectee_actuators(model.getComponent("tau1")); + controller->appendConnectee_actuators(model.getComponent("tau2")); + controller->appendConnectee_actuators(model.getComponent("tau3")); + controller->appendConnectee_actuators(model.getComponent("tau4")); model.addController(controller); diff --git a/OpenSim/Simulation/Control/ControlSetController.cpp b/OpenSim/Simulation/Control/ControlSetController.cpp index 8c5d4d7308..d5f5349f7c 100644 --- a/OpenSim/Simulation/Control/ControlSetController.cpp +++ b/OpenSim/Simulation/Control/ControlSetController.cpp @@ -263,35 +263,32 @@ void ControlSetController::extendFinalizeFromProperties() _controlSet = loadedControlSet; setEnabled(true); } +} +void ControlSetController::extendConnectToModel(Model& model) { const auto& socket = updSocket("actuators"); std::string ext = ".excitation"; for (int i = 0; _controlSet != nullptr && i < _controlSet->getSize(); ++i) { std::string actName = _controlSet->get(i).getName(); - if (actName.length() > ext.length() && - !(actName.compare(actName.length() - ext.length(), ext.length(), ext))) { + if (actName.length() > ext.length() && + !actName.compare( + actName.length() - ext.length(), ext.length(), ext)) { actName.erase(actName.length() - ext.length(), ext.length()); } - // Check if the actuator is already connected to this controller. - const auto& modelActuators = getModel().getComponentList(); - bool foundActuator = false; - for (const auto& actu : modelActuators) { - if (actu.getName() == actName) { - // Do not add the actuator if it is already in the list of - // connectee paths. The connectee paths should be valid at this - // point during deserialization. - if (!socket.hasConnecteePath(actu.getAbsolutePathString())) { - addActuator(actu); - } - foundActuator = true; + // Check that the actuator is connected to the controller. + bool isConnected = false; + for (int iactu = 0; iactu < socket.getNumConnectees(); ++iactu) { + if (socket.getConnectee(iactu).getName() == actName) { + isConnected = true; break; } } - OPENSIM_THROW_IF_FRMOBJ(!foundActuator, Exception, + OPENSIM_THROW_IF_FRMOBJ(!isConnected, Exception, "ControlSetController::extendFinalizeFromProperties: " - "Actuator '{}' in ControlSet '{}' not found in model '{}'.", - actName, _controlSet->getName(), getModel().getName()); + "Actuator '{}' in ControlSet '{}' not connected to controller " + " '{}'.", actName, _controlSet->getName(), getName()); } } + diff --git a/OpenSim/Simulation/Control/ControlSetController.h b/OpenSim/Simulation/Control/ControlSetController.h index 332fc3f920..94bf35cd74 100644 --- a/OpenSim/Simulation/Control/ControlSetController.h +++ b/OpenSim/Simulation/Control/ControlSetController.h @@ -108,9 +108,12 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ControlSetController, Controller); */ void copyData(const ControlSetController &aController); - /// read in ControlSet and update Controller's actuator list + /// read in ControlSet void extendFinalizeFromProperties() override; + /// update the Controller's actuator list + void extendConnectToModel(Model& model) override; + //-------------------------------------------------------------------------- // OPERATORS //-------------------------------------------------------------------------- diff --git a/OpenSim/Simulation/Control/Controller.h b/OpenSim/Simulation/Control/Controller.h index 32f2caca46..cb45c7d2ca 100644 --- a/OpenSim/Simulation/Control/Controller.h +++ b/OpenSim/Simulation/Control/Controller.h @@ -38,10 +38,7 @@ class Actuator; * The defining method of a Controller is its computeControls() method. * @see computeControls() * - * @note Controllers currently do not use the Socket mechanism to locate - * and connect to the Actuators that Controllers depend on. As a result, - * for now, Controllers do not support controlling multiple actuators with - * the same name. + * TODO describe Socket interface, when Actuators should be connected, etc. * * @author Ajay Seth */ @@ -55,7 +52,7 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Controller, ModelComponent); /** Controller is enabled (active) by default. NOTE: Prior to OpenSim 4.0, this property was named **isDisabled**. If **isDisabled** is **true**, **enabled** is **false**. - If **isDisabled** is **false**, **enabled** is **true**. */ + If **isDisabled** is **false**, **enabled** is **true**. */ OpenSim_DECLARE_PROPERTY(enabled, bool, "Flag (true or false) indicating whether or not the controller is " "enabled." ); diff --git a/OpenSim/Simulation/Control/PrescribedController.cpp b/OpenSim/Simulation/Control/PrescribedController.cpp index 8e9fb4efd7..1f8b2c61de 100644 --- a/OpenSim/Simulation/Control/PrescribedController.cpp +++ b/OpenSim/Simulation/Control/PrescribedController.cpp @@ -1,4 +1,4 @@ - /* -------------------------------------------------------------------------- * +/* -------------------------------------------------------------------------- * * OpenSim: PrescribedController.cpp * * -------------------------------------------------------------------------- * * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. * @@ -7,7 +7,7 @@ * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * * through the Warrior Web program. * * * - * Copyright (c) 2005-2017 Stanford University and the Authors * + * Copyright (c) 2005-2023 Stanford University and the Authors * * Author(s): Ajay Seth * * * * Licensed under the Apache License, Version 2.0 (the "License"); you may * @@ -31,63 +31,31 @@ #include #include -//============================================================================= -// STATICS -//============================================================================= - -// This command indicates that any identifier (class, variable, method, etc.) -// defined within the OpenSim namespace can be used in this file without the -// "OpenSim::" prefix. using namespace OpenSim; using namespace std; - - //============================================================================= // CONSTRUCTOR(S) AND DESTRUCTOR //============================================================================= -/* - * Default constructor. - */ -PrescribedController::PrescribedController() : - Controller() -{ +PrescribedController::PrescribedController() : Controller() { setNull(); constructProperties(); } -/* - * Convenience constructor. - */ -PrescribedController:: - PrescribedController(const std::string& controlsFileName, - int interpMethodType) : Controller() -{ +PrescribedController::PrescribedController(const std::string& controlsFileName, + int interpMethodType) : Controller() { setNull(); constructProperties(); set_controls_file(controlsFileName); set_interpolation_method(interpMethodType); } -/* - * Destructor. - */ -PrescribedController::~PrescribedController() -{ -} +PrescribedController::~PrescribedController() = default; -/* - * Set NULL values for all member variables. - */ -void PrescribedController::setNull() -{ +void PrescribedController::setNull() { setAuthors("Ajay Seth"); } -//_____________________________________________________________________________ -/** - * Connect properties to local pointers. - */ void PrescribedController::constructProperties() { constructProperty_ControlFunctions(FunctionSet()); @@ -95,80 +63,83 @@ void PrescribedController::constructProperties() constructProperty_interpolation_method(); } - +//============================================================================= +// MODEL COMPONENT INTERFACE +//============================================================================= void PrescribedController::extendConnectToModel(Model& model) { Super::extendConnectToModel(model); - if(!getProperty_controls_file().empty()){ - Storage controls(get_controls_file()); - const Array& columns = controls.getColumnLabels(); - - int ncols = columns.getSize(); - - int tcol = columns.findIndex("time"); - if(tcol < 0){ - tcol = columns.findIndex("t"); - if(tcol < 0){ - throw Exception("PrescribedController::connectToModel prescribed " - "controls file was not specified as functions of time.", - __FILE__, __LINE__); - } + + // Add prescribed functions for any actuators that were specified by name. + const auto& socket = getSocket("actuators"); + for (const auto& pair : m_prescribedFunctionPairs) { + // Check if the name in the pair is the name or path of an actuator in + // the list Socket. + int actuIndex = getActuatorIndexFromLabel(pair.first); + OPENSIM_THROW_IF_FRMOBJ(actuIndex < 0, Exception, + "Actuator {} is not connected the controller.", pair.first); + + prescribeControlForActuator(actuIndex, pair.second); + } + + // If a controls file was specified, load it and create control functions + // for any actuators that do not already have one. + if(!getProperty_controls_file().empty()) { + + // Load the controls file and find the time column and column labels. + const Storage controls(get_controls_file()); + const Array& columnLabels = controls.getColumnLabels(); + int tcol = columnLabels.findIndex("time"); + if (tcol < 0) { + tcol = columnLabels.findIndex("t"); + OPENSIM_THROW_IF_FRMOBJ(tcol < 0, Exception, "Prescribed controls " + "file was not specified as a function of time.") } int nrows = controls.getSize(); Array time(0.0, nrows); Array data(0.0, nrows); controls.getTimeColumn(time); - FunctionSet& controlFuncs = upd_ControlFunctions(); - const Set& modelActuators = getModel().getActuators(); - - //Set& controllerActuators = updActuators(); - const auto& socket = getSocket("actuators"); - - for(int i=0; i= 0) { - actuator = &modelActuators.get(foundByName); - } else if (getModel().hasComponent(columnLabel)) { - // The column label is an actuator path. - actuator = &getModel().getComponent(columnLabel); - } else { - log_warn("PrescribedController::extendConnectToModel() " - "could not find actuator {} in the model.", - columnLabel); - continue; - } - controls.getDataColumn(controls.getStateIndex(columnLabel), data); - Function* pfunc = - createFunctionFromData(columnLabel, time, data); - // If not already assigned to this controller, assign it. - //int inC = controllerActuators.getIndex(actuator->getName()); - // TODO do I need to check if the actuator is actually connected - // at this point? - int inC = socket.getConnecteePathIndex( - actuator->getAbsolutePathString()); - if(inC >= 0) - prescribeControlForActuator(inC, pfunc); - else{ // add the actuator to the controller's list - addActuator(*actuator); - prescribeControlForActuator(actuator->getName(), pfunc); - } - }// if found in functions, it has already been prescribed - }// end looping through columns - }// if no controls storage specified, do nothing -} + const FunctionSet& controlFuncs = get_ControlFunctions(); + for (int i = 0; i < columnLabels.getSize(); ++i) { + // Skip the time column. + if (i == tcol) continue; + + // If this column does not have an associated control function, we + // need to create one. + const string& columnLabel = columnLabels[i]; + if (!controlFuncs.contains(columnLabel)) { + + // Search for the column label in the model's actuators. + int actuIndex = getActuatorIndexFromLabel(columnLabel); + OPENSIM_THROW_IF_FRMOBJ(actuIndex < 0, Exception, + "The controls file contains column {}, but no Actuator " + "with this label is connected to the controller.", + columnLabel); + + // Create the control function and assign it to the actuator. + controls.getDataColumn( + controls.getStateIndex(columnLabel), data); + Function* controlFunction = createFunctionFromData(columnLabel, + time, data); + prescribeControlForActuator(actuIndex, controlFunction); + } + } + } + // Verify that all actuators have a control function. + const FunctionSet& controlFuncs = get_ControlFunctions(); + OPENSIM_THROW_IF_FRMOBJ(controlFuncs.getSize() != socket.getNumConnectees(), + Exception, "The number of control functions ({}) does not match the " + "number of actuators ({}) connected to the controller.", + controlFuncs.getSize(), socket.getNumConnectees()); +} -// compute the control value for an actuator -void PrescribedController::computeControls(const SimTK::State& s, SimTK::Vector& controls) const -{ +//============================================================================= +// CONTROLLER INTERFACE +//============================================================================= +void PrescribedController::computeControls(const SimTK::State& s, + SimTK::Vector& controls) const { SimTK::Vector actControls(1, 0.0); SimTK::Vector time(1, s.getTime()); @@ -179,54 +150,64 @@ void PrescribedController::computeControls(const SimTK::State& s, SimTK::Vector& } } - //============================================================================= // GET AND SET //============================================================================= - -void PrescribedController:: - prescribeControlForActuator(int index, Function *prescribedFunction) -{ +void PrescribedController::prescribeControlForActuator(int index, + Function* prescribedFunction) { OPENSIM_THROW_IF_FRMOBJ(index < 0, Exception, "Index was " + std::to_string(index) + " but must be nonnegative." ); - int numActuators = getSocket("actuators").getNumConnectees(); - OPENSIM_THROW_IF(index >= numActuators, - IndexOutOfRange, (size_t)index, 0, - (size_t)numActuators - 1); - - if(index >= get_ControlFunctions().getSize()) - upd_ControlFunctions().setSize(index+1); + if (index >= get_ControlFunctions().getSize()) { + upd_ControlFunctions().setSize(index + 1); + } upd_ControlFunctions().set(index, prescribedFunction); } void PrescribedController::prescribeControlForActuator( - const std::string actName, Function *prescribedFunction) -{ - // TODO how to get the actuator path from the name? - const auto& socket = getSocket("actuators"); - int index = socket.getConnecteePathIndex(actName); - OPENSIM_THROW_IF_FRMOBJ(index < 0, Exception, - "PrescribedController does not have {} in its list of actuators to " - "control.", actName) - - prescribeControlForActuator(index, prescribedFunction); + const std::string& actuLabel, Function* prescribedFunction) { + m_prescribedFunctionPairs.emplace_back(actuLabel, prescribedFunction); } -// utility +//============================================================================= +// UTILITY +//============================================================================= Function* PrescribedController::createFunctionFromData(const std::string& name, - const Array& time, const Array& data) -{ + const Array& time, const Array& data) const { int method = 1; - if(!getProperty_interpolation_method().empty()) + if(!getProperty_interpolation_method().empty()) { method = get_interpolation_method(); + } - if(method > 0) + if(method > 0) { return new GCVSpline(method, time.getSize(), &time[0], &data[0], name); - else if(method ==0) + } + + if(method == 0) { return new PiecewiseConstantFunction(time.getSize(), &time[0], &data[0], name); - else - throw Exception("PrescribedController- Invalid interpolation method."); + } + + OPENSIM_THROW_FRMOBJ(Exception, "Invalid interpolation method."); +} + +int PrescribedController::getActuatorIndexFromLabel( + const std::string& actuLabel) const { + const auto& socket = getSocket("actuators"); + for (int i = 0; i < socket.getNumConnectees(); ++i) { + const Actuator& actu = socket.getConnectee(i); + + // Check the actuator name. + if (actu.getName() == actuLabel) { + return i; + } + + // Check the actuator path. + if (actu.getAbsolutePathString() == actuLabel) { + return i; + } + } + + return -1; } diff --git a/OpenSim/Simulation/Control/PrescribedController.h b/OpenSim/Simulation/Control/PrescribedController.h index 249e46b0a5..085f82e860 100644 --- a/OpenSim/Simulation/Control/PrescribedController.h +++ b/OpenSim/Simulation/Control/PrescribedController.h @@ -9,7 +9,7 @@ * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * * through the Warrior Web program. * * * - * Copyright (c) 2005-2017 Stanford University and the Authors * + * Copyright (c) 2005-2023 Stanford University and the Authors * * Author(s): Ajay Seth * * * * Licensed under the Apache License, Version 2.0 (the "License"); you may * @@ -31,36 +31,29 @@ namespace OpenSim { class Function; -//============================================================================= -//============================================================================= /** * PrescribedController is a concrete Controller that specifies functions that * prescribe the control values of its actuators as a function of time. * * @author Ajay Seth */ -//============================================================================= - class OSIMSIMULATION_API PrescribedController : public Controller { OpenSim_DECLARE_CONCRETE_OBJECT(PrescribedController, Controller); +public: //============================================================================= -// DATA +// PROPERTIES //============================================================================= -public: - /** FunctionSet of prescribed controls associated with each actuator */ OpenSim_DECLARE_PROPERTY(ControlFunctions, FunctionSet, - "Functions (one per control) describing the controls for actuators" + "Functions (one per control) describing the controls for actuators " "specified for this controller." ); - /** (Optional) prescribed controls from a storage file */ OpenSim_DECLARE_OPTIONAL_PROPERTY(controls_file, std::string, "Controls storage (.sto) file containing controls for individual " "actuators in the model. Each column label must be either the name " "of an actuator in the model's ForceSet or the absolute path to an " "actuator anywhere in the model."); - /** (Optional) interpolation method for controls in storage. */ OpenSim_DECLARE_OPTIONAL_PROPERTY(interpolation_method, int, "Interpolate the controls file data using piecewise: '0-constant', " "'1-linear', '3-cubic' or '5-quintic' functions."); @@ -68,10 +61,8 @@ OpenSim_DECLARE_CONCRETE_OBJECT(PrescribedController, Controller); //============================================================================= // METHODS //============================================================================= - //-------------------------------------------------------------------------- + // CONSTRUCTION AND DESTRUCTION - //-------------------------------------------------------------------------- -public: /** Default constructor */ PrescribedController(); @@ -84,11 +75,9 @@ OpenSim_DECLARE_CONCRETE_OBJECT(PrescribedController, Controller); int interpMethodType = 1); /** Destructor */ - virtual ~PrescribedController(); + ~PrescribedController() override; - //-------------------------------------------------------------------------- - // CONTROL - //-------------------------------------------------------------------------- + // CONTROLLER INTERFACE /** * Compute the control values for all actuators under the control of this * Controller. @@ -99,43 +88,50 @@ OpenSim_DECLARE_CONCRETE_OBJECT(PrescribedController, Controller); void computeControls(const SimTK::State& s, SimTK::Vector& controls) const override; + // GET AND SET /** * Assign a prescribed control function for the desired actuator identified * by its index. Controller takes ownership of the function. * @param index the actuator's index in the controller's set * @param prescribedFunction the actuator's control function */ - void prescribeControlForActuator(int index, Function *prescribedFunction); + void prescribeControlForActuator(int index, Function* prescribedFunction); /** * Assign a prescribed control function for the desired actuator identified - * by its name. Controller takes ownership of the function. - * @param actName the actuator's name in the controller's set - * @param prescribedFunction the actuator's control function + * by the provided label. The label can be either the name of the actuator, + * or the absolute path to the actuator in the model. Controller takes + * ownership of the function. + * @param actuLabel label for the actuator in the controller + * @param prescribedFunction the actuator's control function */ - void prescribeControlForActuator(const std::string actName, - Function *prescribedFunction); + void prescribeControlForActuator(const std::string& actuLabel, + Function* prescribedFunction); protected: - /** Model component interface */ + // MODEL COMPONENT INTERFACE void extendConnectToModel(Model& model) override; + private: - // construct and initialize properties + // Construct and initialize properties. void constructProperties(); - // utility + // Utility functions. Function* createFunctionFromData(const std::string& name, - const Array& time, const Array& data); + const Array& time, const Array& data) const; + int getActuatorIndexFromLabel(const std::string& actuLabel) const; // This method sets all member variables to default (e.g., NULL) values. void setNull(); -//============================================================================= -}; // END of class PrescribedController + // This member variable is used to store any function prescribed + // based on the actuator's name or path. + std::vector> + m_prescribedFunctionPairs; -}; //namespace -//============================================================================= -//============================================================================= +}; // class PrescribedController + +} // namespace OpenSim #endif // OPENSIM_PRESCRIBED_CONTROLLER_H_ diff --git a/OpenSim/Tools/Test/testControllers.cpp b/OpenSim/Tools/Test/testControllers.cpp index 7cf00a7b63..cc70c51bbe 100644 --- a/OpenSim/Tools/Test/testControllers.cpp +++ b/OpenSim/Tools/Test/testControllers.cpp @@ -125,13 +125,16 @@ void testControlSetControllerOnBlock() actuatorControls.setControlValues(initialTime, controlForce); actuatorControls.setControlValues(finalTime, controlForce); // Create a control set controller that simply applies controls from a ControlSet - ControlSetController actuatorController; + auto* actuatorController = new ControlSetController(); + actuatorController->addActuator( + osimModel.getComponent("/forceset/actuator")); // Make a copy and set it on the ControlSetController as it takes ownership of the // ControlSet passed in - actuatorController.setControlSet((ControlSet*)Object::SafeCopy(&actuatorControls)); + actuatorController->setControlSet( + (ControlSet*)Object::SafeCopy(&actuatorControls)); // add the controller to the model - osimModel.addController(&actuatorController); + osimModel.addController(actuatorController); // Initialize the system and get the state representing the state system SimTK::State& si = osimModel.initSystem(); @@ -336,7 +339,7 @@ void testPrescribedControllerFromFile(const std::string& modelFile, ControlSetController csc; ControlSet* cs = new ControlSet(controlsFile); csc.setControlSet(cs); - + csc.setActuators(osimModel.updActuators()); // add the controller to the model osimModel.addController(&csc); @@ -376,9 +379,11 @@ void testPrescribedControllerFromFile(const std::string& modelFile, //************* Rerun with a PrescribedController ***********************/ PrescribedController prescribed(outfileName, 1); + prescribed.setActuators(osimModel.updActuators()); // add the controller to the model osimModel.addController(&prescribed); + osimModel.finalizeConnections(); osimModel.print("TestPrescribedController.osim"); // Initialize the system and get the state representing the state system From f75caf5c86f1a7ada4d46fc7bb915ebaa493660e Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 12 Jan 2024 10:09:05 -0800 Subject: [PATCH 18/48] Figuring out how to deal with PrescribedController --- Bindings/Java/tests/TestModelBuilding.java | 3 +- .../LuxoMuscle_create_and_simulate.cpp | 12 ++-- OpenSim/Sandbox/Moco/sandboxSandbox.cpp | 1 + OpenSim/Simulation/Control/Controller.cpp | 9 --- OpenSim/Simulation/Control/Controller.h | 10 --- .../Control/PrescribedController.cpp | 39 +++++++---- .../Simulation/Control/PrescribedController.h | 7 +- OpenSim/Simulation/Test/testManager.cpp | 3 +- OpenSim/Tests/Wrapping/testWrapping.cpp | 6 +- OpenSim/Tools/CMCTool.cpp | 67 ++++++++++++------- OpenSim/Tools/CMCTool.h | 6 +- OpenSim/Tools/Test/testControllers.cpp | 4 +- 12 files changed, 93 insertions(+), 74 deletions(-) diff --git a/Bindings/Java/tests/TestModelBuilding.java b/Bindings/Java/tests/TestModelBuilding.java index fdf9cce6c9..58e8c970ff 100644 --- a/Bindings/Java/tests/TestModelBuilding.java +++ b/Bindings/Java/tests/TestModelBuilding.java @@ -32,8 +32,7 @@ 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.getAbsolutePathString(), testFunction); System.gc(); // Request gc could free testFunction and crash arm.addBody(hum); arm.addBody(rad); diff --git a/OpenSim/Examples/ExampleLuxoMuscle/LuxoMuscle_create_and_simulate.cpp b/OpenSim/Examples/ExampleLuxoMuscle/LuxoMuscle_create_and_simulate.cpp index 81ce50c5df..4deea63ea2 100644 --- a/OpenSim/Examples/ExampleLuxoMuscle/LuxoMuscle_create_and_simulate.cpp +++ b/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); diff --git a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp index 93491f8e0f..33fc794d99 100644 --- a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp +++ b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp @@ -47,6 +47,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(TestController, Controller); int index = socket.getConnecteePathIndex(actuPath); log_cout("extendConnectToModel: connectee path index = {}", index); + log_cout("extendConnectToModel: num connectees = {}", socket.getNumConnectees()); } void extendAddToSystem(SimTK::MultibodySystem& system) const override { diff --git a/OpenSim/Simulation/Control/Controller.cpp b/OpenSim/Simulation/Control/Controller.cpp index 1239647655..1794aeef0f 100644 --- a/OpenSim/Simulation/Control/Controller.cpp +++ b/OpenSim/Simulation/Control/Controller.cpp @@ -84,15 +84,6 @@ void Controller::updateFromXMLNode(SimTK::Xml::Element& node, Super::updateFromXMLNode(node, versionNumber); } -void Controller::extendConnectToModel(Model& model) { - Super::extendConnectToModel(model); - // TODO check if all actuators in connectee paths are in the model? -} - -void Controller::extendAddToSystem(SimTK::MultibodySystem& system) const { - Super::extendAddToSystem(system); -} - //============================================================================= // GET AND SET //============================================================================= diff --git a/OpenSim/Simulation/Control/Controller.h b/OpenSim/Simulation/Control/Controller.h index cb45c7d2ca..e034911953 100644 --- a/OpenSim/Simulation/Control/Controller.h +++ b/OpenSim/Simulation/Control/Controller.h @@ -119,16 +119,6 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Controller, ModelComponent); } protected: - - /** Model component interface that permits the controller to be "wired" up - * to its actuators. Subclasses can override to perform additional setup. */ - void extendConnectToModel(Model& model) override; - - /** Model component interface that creates underlying computational - * components in the SimTK::MultibodySystem. This includes adding states, - * creating measures, etc., required by the controller. */ - void extendAddToSystem(SimTK::MultibodySystem& system) const override; - /** Only a Controller can set its number of controls based on its * actuators. */ void setNumControls(int numControls) { _numControls = numControls; } diff --git a/OpenSim/Simulation/Control/PrescribedController.cpp b/OpenSim/Simulation/Control/PrescribedController.cpp index 1f8b2c61de..65e7a382f7 100644 --- a/OpenSim/Simulation/Control/PrescribedController.cpp +++ b/OpenSim/Simulation/Control/PrescribedController.cpp @@ -72,16 +72,18 @@ void PrescribedController::extendConnectToModel(Model& model) // Add prescribed functions for any actuators that were specified by name. const auto& socket = getSocket("actuators"); - for (const auto& pair : m_prescribedFunctionPairs) { + for (auto& pair : m_prescribedFunctionPairs) { // Check if the name in the pair is the name or path of an actuator in // the list Socket. int actuIndex = getActuatorIndexFromLabel(pair.first); OPENSIM_THROW_IF_FRMOBJ(actuIndex < 0, Exception, "Actuator {} is not connected the controller.", pair.first); - prescribeControlForActuator(actuIndex, pair.second); + prescribeControlForActuator(actuIndex, &pair.second); } + auto& controlFuncs = upd_ControlFunctions(); + // If a controls file was specified, load it and create control functions // for any actuators that do not already have one. if(!getProperty_controls_file().empty()) { @@ -153,21 +155,30 @@ void PrescribedController::computeControls(const SimTK::State& s, //============================================================================= // GET AND SET //============================================================================= -void PrescribedController::prescribeControlForActuator(int index, - Function* prescribedFunction) { - OPENSIM_THROW_IF_FRMOBJ(index < 0, - Exception, "Index was " + std::to_string(index) + - " but must be nonnegative." ); - - if (index >= get_ControlFunctions().getSize()) { - upd_ControlFunctions().setSize(index + 1); - } - upd_ControlFunctions().set(index, prescribedFunction); -} +// void PrescribedController::prescribeControlForActuator(int index, +// Function* prescribedFunction) { +// OPENSIM_THROW_IF_FRMOBJ(index < 0, +// Exception, "Index was " + std::to_string(index) + +// " but must be nonnegative." ); +// +// if (index >= get_ControlFunctions().getSize()) { +// upd_ControlFunctions().setSize(index + 1); +// } +// upd_ControlFunctions().set(index, prescribedFunction); +// } void PrescribedController::prescribeControlForActuator( const std::string& actuLabel, Function* prescribedFunction) { - m_prescribedFunctionPairs.emplace_back(actuLabel, prescribedFunction); + prescribedFunction->setName(actuLabel); + FunctionSet& controlFuncs = upd_ControlFunctions(); + if (controlFuncs.contains(actuLabel)) { + const int index = controlFuncs.getIndex(actuLabel); + controlFuncs.set(index, prescribedFunction); + } else { + const int size = controlFuncs.getSize(); + controlFuncs.setSize(size + 1); + controlFuncs.set(size, prescribedFunction); + } } //============================================================================= diff --git a/OpenSim/Simulation/Control/PrescribedController.h b/OpenSim/Simulation/Control/PrescribedController.h index 085f82e860..e280a35d35 100644 --- a/OpenSim/Simulation/Control/PrescribedController.h +++ b/OpenSim/Simulation/Control/PrescribedController.h @@ -95,7 +95,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(PrescribedController, Controller); * @param index the actuator's index in the controller's set * @param prescribedFunction the actuator's control function */ - void prescribeControlForActuator(int index, Function* prescribedFunction); + // void prescribeControlForActuator(int index, Function* prescribedFunction); /** * Assign a prescribed control function for the desired actuator identified @@ -124,11 +124,6 @@ OpenSim_DECLARE_CONCRETE_OBJECT(PrescribedController, Controller); // This method sets all member variables to default (e.g., NULL) values. void setNull(); - // This member variable is used to store any function prescribed - // based on the actuator's name or path. - std::vector> - m_prescribedFunctionPairs; - }; // class PrescribedController } // namespace OpenSim diff --git a/OpenSim/Simulation/Test/testManager.cpp b/OpenSim/Simulation/Test/testManager.cpp index b2c763a041..e94f814e37 100644 --- a/OpenSim/Simulation/Test/testManager.cpp +++ b/OpenSim/Simulation/Test/testManager.cpp @@ -285,7 +285,8 @@ void testExcitationUpdatesWithManager() PrescribedController* controller = new PrescribedController(); controller->addActuator(muscleSet.get(0)); Constant* fn = new Constant(0); - controller->prescribeControlForActuator(0, fn); + controller->prescribeControlForActuator( + muscleSet.get(0).getAbsolutePathString(), fn); arm.addController(controller); SimTK::State& state = arm.initSystem(); diff --git a/OpenSim/Tests/Wrapping/testWrapping.cpp b/OpenSim/Tests/Wrapping/testWrapping.cpp index e70f6de394..a78b809465 100644 --- a/OpenSim/Tests/Wrapping/testWrapping.cpp +++ b/OpenSim/Tests/Wrapping/testWrapping.cpp @@ -82,11 +82,13 @@ namespace { // Create a PrescribedController that simply applies a function of the // force. PrescribedController actuController; - actuController.setActuators(model.updActuators()); + const auto& actuatorsSet = model.getActuators(); + actuController.setActuators(actuatorsSet); const auto& socket = actuController.getSocket("actuators"); for (int i = 0; i < socket.getNumConnectees(); i++) { actuController.prescribeControlForActuator( - i, new Constant(activation)); + actuatorsSet.get(i).getAbsolutePathString(), + new Constant(activation)); } // Add the controller to the model. We need to call disownAllComponents diff --git a/OpenSim/Tools/CMCTool.cpp b/OpenSim/Tools/CMCTool.cpp index 41a47a4216..f99a38ab76 100644 --- a/OpenSim/Tools/CMCTool.cpp +++ b/OpenSim/Tools/CMCTool.cpp @@ -1089,44 +1089,65 @@ void CMCTool::setOriginalForceSet(const ForceSet &aForceSet) { /* Get the Set of model actuators for CMC that exclude user specified Actuators */ -Set CMCTool:: - getActuatorsForCMC(const Array &actuatorsByNameOrGroup) -{ - Set actuatorsForCMC = _model->getActuators(); - for (int i=actuatorsForCMC.getSize()-1; i>0; i--){ - if(!actuatorsForCMC.get(i).get_appliesForce()) - actuatorsForCMC.remove(i); +Array_> CMCTool::getActuatorsForCMC( + const Array& actuatorsByNameOrGroup) { + + const Set& actuatorsForCMC = _model->getActuators(); + std::vector actuatorsToSkip; + for (int i=actuatorsForCMC.getSize()-1; i>0; i--) { + if (!actuatorsForCMC.get(i).get_appliesForce()) { + actuatorsToSkip.push_back( + actuatorsForCMC.get(i).getAbsolutePathString()); + } } Array groupNames; actuatorsForCMC.getGroupNames(groupNames); /* The search for individual group or force names IS case-sensitive BUT keywords are not*/ - for(int i=0; i 0){ + // So check if name is a group name first + if (groupNames.getSize() > 0) { k = groupNames.findIndex(actuatorsByNameOrGroup[i]); - if(k > -1){ //found + if (k > -1) { // found const ObjectGroup* group = actuatorsForCMC.getGroup(k); Array members = group->getMembers(); - for(int j=0; j(members[j])) { + actuatorsToSkip.push_back( + actu->getAbsolutePathString()); + } + } } - } //otherwise, check for an individual actuator - else{ + } // otherwise, check for an individual actuator + else { k = actuatorsForCMC.getIndex(actuatorsByNameOrGroup[i]); - if(k > -1){ //found - actuatorsForCMC.remove(k); + if (k > -1) { // found + actuatorsToSkip.push_back( + actuatorsForCMC.get(k).getAbsolutePathString()); } } // No actuator(s) by group or individual name was found - if(k < 0) + if (k < 0) log_warn("CMCTool: Could not find actuator or group named '{}' to " - "be excluded from CMC.", actuatorsByNameOrGroup[i]); + "be excluded from CMC.", + actuatorsByNameOrGroup[i]); + } + + const int numActu = actuatorsForCMC.getSize(); + Array_> actuatorsForCMCRefs(numActu); + for (int i = 0; i < numActu; ++i) { + const auto& actuPath = actuatorsForCMC.get(i).getAbsolutePathString(); + if (std::find(actuatorsToSkip.begin(), actuatorsToSkip.end(), actuPath) != + actuatorsToSkip.end()) { + continue; + } + const auto& actuator = _model->getComponent(actuPath); + actuatorsForCMCRefs[i] = &actuator; } - return actuatorsForCMC; + return actuatorsForCMCRefs; } diff --git a/OpenSim/Tools/CMCTool.h b/OpenSim/Tools/CMCTool.h index 3cecf73e0f..c38051ff05 100644 --- a/OpenSim/Tools/CMCTool.h +++ b/OpenSim/Tools/CMCTool.h @@ -140,9 +140,11 @@ OpenSim_DECLARE_CONCRETE_OBJECT(CMCTool, AbstractTool); private: void setNull(); void setupProperties(); - /* Get the Set of model actuators for CMC that exclude user specified Actuators */ - Set getActuatorsForCMC(const Array &actuatorsByNameOrGroup); + /** Get the array of model actuators for CMC that exclude user specified + Actuators */ + SimTK::Array_> + getActuatorsForCMC(const Array &actuatorsByNameOrGroup); //-------------------------------------------------------------------------- // OPERATORS diff --git a/OpenSim/Tools/Test/testControllers.cpp b/OpenSim/Tools/Test/testControllers.cpp index cc70c51bbe..e84869cc94 100644 --- a/OpenSim/Tools/Test/testControllers.cpp +++ b/OpenSim/Tools/Test/testControllers.cpp @@ -217,7 +217,9 @@ void testPrescribedControllerOnBlock(bool enabled) PrescribedController actuatorController; actuatorController.setName("testPrescribedController"); actuatorController.setActuators(osimModel.updActuators()); - actuatorController.prescribeControlForActuator(0, new Constant(controlForce)); + actuatorController.prescribeControlForActuator( + osimModel.getActuators().get(0).getAbsolutePathString(), + new Constant(controlForce)); actuatorController.setEnabled(enabled); // add the controller to the model From fdf9bd7c562d6b49cfc067e11d5aebc9d21fa962 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 19 Jan 2024 10:24:49 -0800 Subject: [PATCH 19/48] Refactor PrescribedController --- OpenSim/Simulation/Control/Controller.cpp | 2 +- .../Control/PrescribedController.cpp | 61 ++++++++++--------- .../Simulation/Control/PrescribedController.h | 15 +++-- 3 files changed, 40 insertions(+), 38 deletions(-) diff --git a/OpenSim/Simulation/Control/Controller.cpp b/OpenSim/Simulation/Control/Controller.cpp index 1794aeef0f..82e9449a43 100644 --- a/OpenSim/Simulation/Control/Controller.cpp +++ b/OpenSim/Simulation/Control/Controller.cpp @@ -111,7 +111,7 @@ void Controller::setActuators( } void Controller::addActuator(const Actuator& actuator) { - appendConnectee_actuators(actuator); + appendSocketConnectee_actuators(actuator); } SimTK::Array_> diff --git a/OpenSim/Simulation/Control/PrescribedController.cpp b/OpenSim/Simulation/Control/PrescribedController.cpp index 65e7a382f7..9e3b12505f 100644 --- a/OpenSim/Simulation/Control/PrescribedController.cpp +++ b/OpenSim/Simulation/Control/PrescribedController.cpp @@ -69,20 +69,7 @@ void PrescribedController::constructProperties() void PrescribedController::extendConnectToModel(Model& model) { Super::extendConnectToModel(model); - - // Add prescribed functions for any actuators that were specified by name. const auto& socket = getSocket("actuators"); - for (auto& pair : m_prescribedFunctionPairs) { - // Check if the name in the pair is the name or path of an actuator in - // the list Socket. - int actuIndex = getActuatorIndexFromLabel(pair.first); - OPENSIM_THROW_IF_FRMOBJ(actuIndex < 0, Exception, - "Actuator {} is not connected the controller.", pair.first); - - prescribeControlForActuator(actuIndex, &pair.second); - } - - auto& controlFuncs = upd_ControlFunctions(); // If a controls file was specified, load it and create control functions // for any actuators that do not already have one. @@ -110,7 +97,7 @@ void PrescribedController::extendConnectToModel(Model& model) // If this column does not have an associated control function, we // need to create one. const string& columnLabel = columnLabels[i]; - if (!controlFuncs.contains(columnLabel)) { + if (!_actuLabelsToControlFunctionIndexMap.count(columnLabel)) { // Search for the column label in the model's actuators. int actuIndex = getActuatorIndexFromLabel(columnLabel); @@ -124,11 +111,38 @@ void PrescribedController::extendConnectToModel(Model& model) controls.getStateIndex(columnLabel), data); Function* controlFunction = createFunctionFromData(columnLabel, time, data); - prescribeControlForActuator(actuIndex, controlFunction); + prescribeControlForActuator(columnLabel, controlFunction); } } } + // Populate the _actuIndexToControlFunctionIndexMap. + for (const auto& pair : _actuLabelsToControlFunctionIndexMap) { + int actuIndex = getActuatorIndexFromLabel(pair.first); + if (actuIndex < 0) { + OPENSIM_THROW_FRMOBJ(Exception, + "Actuator {} was not found in the model.", pair.first) + } + _actuIndexToControlFunctionIndexMap[actuIndex] = pair.second; + } + + // Check for actuators with multiple control functions. + std::vector uniqueValues; + for (const auto& pair : _actuIndexToControlFunctionIndexMap) { + int value = pair.second; + if (std::find(uniqueValues.begin(), uniqueValues.end(), value) != + uniqueValues.end()) { + OPENSIM_THROW_FRMOBJ(Exception, + "Expected actuator {} to have one control function " + "assigned, but multiple control functions were detected. " + "This may have occurred because a control function was " + "specified by actuator name and by actuator path.", + socket.getConnectee(pair.first).getAbsolutePathString()) + } else { + uniqueValues.push_back(value); + } + } + // Verify that all actuators have a control function. const FunctionSet& controlFuncs = get_ControlFunctions(); OPENSIM_THROW_IF_FRMOBJ(controlFuncs.getSize() != socket.getNumConnectees(), @@ -155,29 +169,18 @@ void PrescribedController::computeControls(const SimTK::State& s, //============================================================================= // GET AND SET //============================================================================= -// void PrescribedController::prescribeControlForActuator(int index, -// Function* prescribedFunction) { -// OPENSIM_THROW_IF_FRMOBJ(index < 0, -// Exception, "Index was " + std::to_string(index) + -// " but must be nonnegative." ); -// -// if (index >= get_ControlFunctions().getSize()) { -// upd_ControlFunctions().setSize(index + 1); -// } -// upd_ControlFunctions().set(index, prescribedFunction); -// } - void PrescribedController::prescribeControlForActuator( const std::string& actuLabel, Function* prescribedFunction) { prescribedFunction->setName(actuLabel); FunctionSet& controlFuncs = upd_ControlFunctions(); - if (controlFuncs.contains(actuLabel)) { - const int index = controlFuncs.getIndex(actuLabel); + if (_actuLabelsToControlFunctionIndexMap.count(actuLabel)) { + const int index = _actuLabelsToControlFunctionIndexMap.at(actuLabel); controlFuncs.set(index, prescribedFunction); } else { const int size = controlFuncs.getSize(); controlFuncs.setSize(size + 1); controlFuncs.set(size, prescribedFunction); + _actuLabelsToControlFunctionIndexMap[actuLabel] = size; } } diff --git a/OpenSim/Simulation/Control/PrescribedController.h b/OpenSim/Simulation/Control/PrescribedController.h index e280a35d35..44c86a1028 100644 --- a/OpenSim/Simulation/Control/PrescribedController.h +++ b/OpenSim/Simulation/Control/PrescribedController.h @@ -89,14 +89,6 @@ OpenSim_DECLARE_CONCRETE_OBJECT(PrescribedController, Controller); SimTK::Vector& controls) const override; // GET AND SET - /** - * Assign a prescribed control function for the desired actuator identified - * by its index. Controller takes ownership of the function. - * @param index the actuator's index in the controller's set - * @param prescribedFunction the actuator's control function - */ - // void prescribeControlForActuator(int index, Function* prescribedFunction); - /** * Assign a prescribed control function for the desired actuator identified * by the provided label. The label can be either the name of the actuator, @@ -108,6 +100,9 @@ OpenSim_DECLARE_CONCRETE_OBJECT(PrescribedController, Controller); void prescribeControlForActuator(const std::string& actuLabel, Function* prescribedFunction); + [[deprecated("this method no longer does anything")]] + void prescribeControlForActuator(int index, Function* prescribedFunction) {} + protected: // MODEL COMPONENT INTERFACE void extendConnectToModel(Model& model) override; @@ -124,6 +119,10 @@ OpenSim_DECLARE_CONCRETE_OBJECT(PrescribedController, Controller); // This method sets all member variables to default (e.g., NULL) values. void setNull(); + // Member variables. + std::unordered_map _actuLabelsToControlFunctionIndexMap; + std::unordered_map _actuIndexToControlFunctionIndexMap; + }; // class PrescribedController } // namespace OpenSim From fc174e8a855325af05e7b8823262de41a3b13018 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 19 Jan 2024 10:36:25 -0800 Subject: [PATCH 20/48] Update changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index 3a4ceee5aa..0ee391878f 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -21,6 +21,7 @@ new `getConnectee` overloads that take an index to a desired object in the list querying a component's outputs by name (#3673) - The XMLDocument that is held within OpenSim::Object is now reference-counted, to help ensure it is freed (e.g. when an exception is thrown) +- `Controller` now manages the list of controlled actuators using a list `Socket` instead of a `Set` (#3683) v4.5 ==== From d99a811b8084d405f2951b8e160781832a49c625 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 19 Jan 2024 11:37:20 -0800 Subject: [PATCH 21/48] Fix changelog typos // remove TODOs --- CHANGELOG.md | 4 +- OpenSim/Sandbox/Moco/sandboxSandbox.cpp | 20 ++++++--- OpenSim/Simulation/Control/Controller.cpp | 8 +++- OpenSim/Simulation/Control/Controller.h | 41 ++++++++++++++++++- .../Simulation/Control/PrescribedController.h | 5 +++ OpenSim/Tools/CorrectionController.cpp | 1 - OpenSim/Tools/VectorFunctionForActuators.cpp | 4 +- 7 files changed, 69 insertions(+), 14 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 0ee391878f..c2380ad335 100644 --- a/CHANGELOG.md +++ b/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` implementation, making it usable in (e.g.) `std::unordered_map` diff --git a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp index 33fc794d99..b29a04b4b0 100644 --- a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp +++ b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp @@ -77,11 +77,21 @@ int main() { auto* controller = new TestController(); controller->setActuatorPath("/tau3"); - controller->appendConnectee_actuators(model.getComponent("tau0")); - controller->appendConnectee_actuators(model.getComponent("tau1")); - controller->appendConnectee_actuators(model.getComponent("tau2")); - controller->appendConnectee_actuators(model.getComponent("tau3")); - controller->appendConnectee_actuators(model.getComponent("tau4")); + //controller->setActuators(model.getComponentList()); + + const auto& actu1 = model.getComponent("/tau1"); + const auto& actu2 = model.getComponent("/tau2"); + const auto& actu3 = model.getComponent("/tau3"); + const auto& actu4 = model.getComponent("/tau4"); + const auto& actu5 = model.getComponent("/tau5"); + + SimTK::Array_> actuators(5); + actuators[0] = &actu1; + actuators[1] = &actu2; + actuators[2] = &actu3; + actuators[3] = &actu4; + actuators[4] = &actu5; + controller->setActuators(actuators); model.addController(controller); diff --git a/OpenSim/Simulation/Control/Controller.cpp b/OpenSim/Simulation/Control/Controller.cpp index 82e9449a43..2451640811 100644 --- a/OpenSim/Simulation/Control/Controller.cpp +++ b/OpenSim/Simulation/Control/Controller.cpp @@ -110,6 +110,13 @@ void Controller::setActuators( } } +void Controller::setActuators(const ComponentList& actuators) { + updSocket("actuators").disconnect(); + for (const auto& actu : actuators) { + addActuator(actu); + } +} + void Controller::addActuator(const Actuator& actuator) { appendSocketConnectee_actuators(actuator); } @@ -126,4 +133,3 @@ Controller::getActuators() const { return actuators; } - diff --git a/OpenSim/Simulation/Control/Controller.h b/OpenSim/Simulation/Control/Controller.h index e034911953..23f419f81f 100644 --- a/OpenSim/Simulation/Control/Controller.h +++ b/OpenSim/Simulation/Control/Controller.h @@ -35,10 +35,46 @@ class Actuator; * Controller is an abstract ModelComponent that defines the interface for * an OpenSim Controller. A controller computes and sets the values of the * controls for the actuators under its control. - * The defining method of a Controller is its computeControls() method. + * + * The defining method of a Controller is its computeControls() method. All + * concrete controllers must implement this method. * @see computeControls() * - * TODO describe Socket interface, when Actuators should be connected, etc. + * Actuators can be connected to a Controller via the list Socket `actuators`. + * Connection can be made via the `addActuator()` convenience method or through + * the Socket directly: + * + * @code{.cpp} + * // Add an actuator to the controller. + * const auto& actuator = model.getComponent("/path/to/actuator"); + * controller.addActuator(actuator); + * + * // Connect an actuator to the controller via the actuators Socket. + * controller.appendSocketConnectee_actuators(actuator); + * @endcode + * + * Multiple actuators can be connected to a Controller via the `setActuators()` + * convenience methods: + * + * @code{.cpp} + * // Add a Model's Set of Actuators to the controller. + * controller.setActuators(model.getActuators()); + * + * // Add a ComponentList of Actuators to the controller. + * controller.setActuators(model.getComponentList()); + * + * // Add an array of specific Actuator reference pointers to the controller. + * const auto& actuator1 = model.getComponent("/path/to/actuator1"); + * const auto& actuator2 = model.getComponent("/path/to/actuator2"); + * SimTK::Array_> actuators(2); + * actuators[0] = &actuator1; + * actuators[1] = &actuator2; + * controller.setActuators(actuators); + * @endcode + * + * @note Prior to OpenSim 4.6, controlled actuators were managed via the list + * Property `actuator_list`. This interface is no longer supported, all + * actuators must be connected via the `actuators` list Socket. * * @author Ajay Seth */ @@ -92,6 +128,7 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Controller, ModelComponent); void setActuators(const Set& actuators); void setActuators( const SimTK::Array_>& actuators); + void setActuators(const ComponentList& actuators); /** Add to the current set of actuators. */ void addActuator(const Actuator& actuator); diff --git a/OpenSim/Simulation/Control/PrescribedController.h b/OpenSim/Simulation/Control/PrescribedController.h index 44c86a1028..c7af93570f 100644 --- a/OpenSim/Simulation/Control/PrescribedController.h +++ b/OpenSim/Simulation/Control/PrescribedController.h @@ -35,6 +35,11 @@ class Function; * PrescribedController is a concrete Controller that specifies functions that * prescribe the control values of its actuators as a function of time. * + * @note Prior to OpenSim 4.6, PrescribedController support setting a prescribed + * control based on the actuator's index in the `ControlFunctions` + * property. This interface is deprecated and will be removed in a future + * release. + * * @author Ajay Seth */ class OSIMSIMULATION_API PrescribedController : public Controller { diff --git a/OpenSim/Tools/CorrectionController.cpp b/OpenSim/Tools/CorrectionController.cpp index 515ff79349..25aad2eefd 100644 --- a/OpenSim/Tools/CorrectionController.cpp +++ b/OpenSim/Tools/CorrectionController.cpp @@ -293,7 +293,6 @@ void CorrectionController::extendConnectToModel(Model& model) // We're only using CoordinateActuators here, so the number of actuators // should always match the number of controls. - // TODO is the number of connectees correct at this point? setNumControls(getNumActuators()); log_info("CorrectionController::extendConnectToModel(): " diff --git a/OpenSim/Tools/VectorFunctionForActuators.cpp b/OpenSim/Tools/VectorFunctionForActuators.cpp index 090267dcc6..9d3409cc88 100644 --- a/OpenSim/Tools/VectorFunctionForActuators.cpp +++ b/OpenSim/Tools/VectorFunctionForActuators.cpp @@ -266,10 +266,8 @@ evaluate(const SimTK::State& s, const double *aX, double *rF) ts.initialize(actSysState); ts.stepTo(_tf); - // TODO check that the socket actuators are in the correct order for the - // target forces - const auto& socket = controller.getSocket("actuators"); // Vector function values + const auto& socket = controller.getSocket("actuators"); int j = 0; for(i=0;i(&socket.getConnectee(i)); From 2b9151531f861d721ff2e28d7c590539f4893d1c Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 19 Jan 2024 15:34:12 -0800 Subject: [PATCH 22/48] Support loading older XMl files --- OpenSim/CMakeLists.txt | 2 +- OpenSim/Common/XMLDocument.cpp | 5 +- OpenSim/Sandbox/Moco/sandboxSandbox.cpp | 88 ++------------ OpenSim/Simulation/Control/Controller.cpp | 49 +++++++- OpenSim/Simulation/Control/Controller.h | 9 ++ OpenSim/Simulation/Test/testInitState.cpp | 1 + OpenSim/Tools/Test/testControllers.cpp | 137 +++++++++++++++------- 7 files changed, 165 insertions(+), 126 deletions(-) diff --git a/OpenSim/CMakeLists.txt b/OpenSim/CMakeLists.txt index 8ba2fcc16e..8d53dedd8f 100644 --- a/OpenSim/CMakeLists.txt +++ b/OpenSim/CMakeLists.txt @@ -10,6 +10,6 @@ add_subdirectory(Moco) add_subdirectory(Examples) add_subdirectory(Tests) -add_subdirectory(Sandbox) +#add_subdirectory(Sandbox) install(FILES OpenSim.h DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/OpenSim") diff --git a/OpenSim/Common/XMLDocument.cpp b/OpenSim/Common/XMLDocument.cpp index 3514bb3dda..9ae57cec66 100644 --- a/OpenSim/Common/XMLDocument.cpp +++ b/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) //============================================================================= diff --git a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp index b29a04b4b0..8de77ca7ce 100644 --- a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp +++ b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp @@ -19,83 +19,19 @@ // This file provides a way to easily prototype or test temporary snippets of // code during development. -#include -#include +#include using namespace OpenSim; -class TestController : public Controller { -OpenSim_DECLARE_CONCRETE_OBJECT(TestController, Controller); -public: - void computeControls(const SimTK::State& s, - SimTK::Vector& controls) const override { - log_cout("Computing controls..."); - } - - void setActuatorPath(const std::string& path) { - m_actuatorPath = path; - } - - const std::string& getActuatorPath() const { - return m_actuatorPath; - } - -protected: - void extendConnectToModel(Model& model) override { - const auto& socket = getSocket("actuators"); - const auto& actuPath = getActuatorPath(); - int index = socket.getConnecteePathIndex(actuPath); - log_cout("extendConnectToModel: connectee path index = {}", index); - - log_cout("extendConnectToModel: num connectees = {}", socket.getNumConnectees()); - } - - void extendAddToSystem(SimTK::MultibodySystem& system) const override { - const auto& socket = getSocket("actuators"); - const auto& actuPath = getActuatorPath(); - int index = socket.getConnecteePathIndex(actuPath); - log_cout("extendAddToSystem: connectee path index = {}", index); - } - - void extendFinalizeFromProperties() override { - - const auto& socket = getSocket("actuators"); - const auto& actuPath = getActuatorPath(); - int index = socket.getConnecteePathIndex(actuPath); - log_cout("extendAddToSystem: connectee path index = {}", index); - - } - -private: - std::string m_actuatorPath; - -}; - int main() { - - Model model = ModelFactory::createNLinkPendulum(5); - - auto* controller = new TestController(); - controller->setActuatorPath("/tau3"); - //controller->setActuators(model.getComponentList()); - - const auto& actu1 = model.getComponent("/tau1"); - const auto& actu2 = model.getComponent("/tau2"); - const auto& actu3 = model.getComponent("/tau3"); - const auto& actu4 = model.getComponent("/tau4"); - const auto& actu5 = model.getComponent("/tau5"); - - SimTK::Array_> actuators(5); - actuators[0] = &actu1; - actuators[1] = &actu2; - actuators[2] = &actu3; - actuators[3] = &actu4; - actuators[4] = &actu5; - controller->setActuators(actuators); - - model.addController(controller); - - model.initSystem(); - - return EXIT_SUCCESS; -} + // TODO Logger::setLevel(Logger::Level::Debug); + MocoTrack track; + ModelProcessor modelProcessor("DeMers_mod_noarms_welds_4.0.osim"); + modelProcessor.append(ModOpReplaceMusclesWithDeGrooteFregly2016()); + modelProcessor.append(ModOpIgnoreTendonCompliance()); + track.setModel(modelProcessor); + track.setStatesReference({"r_SLD_mean_coords.sto"}); + track.set_allow_unused_references(true); + MocoSolution solution = track.solve(); + return EXIT_SUCCESS; +} \ No newline at end of file diff --git a/OpenSim/Simulation/Control/Controller.cpp b/OpenSim/Simulation/Control/Controller.cpp index 2451640811..2afbf691d2 100644 --- a/OpenSim/Simulation/Control/Controller.cpp +++ b/OpenSim/Simulation/Control/Controller.cpp @@ -58,12 +58,12 @@ void Controller::constructProperties() { } //============================================================================= -// COMPONENT INTERFACE +// MODEL COMPONENT INTERFACE //============================================================================= void Controller::updateFromXMLNode(SimTK::Xml::Element& node, int versionNumber) { - if(versionNumber < XMLDocument::getLatestVersion()) { - if(versionNumber < 30509) { + if (versionNumber < XMLDocument::getLatestVersion()) { + if (versionNumber < 30509) { // Rename property 'isDisabled' to 'enabled' and // negate the contained value. std::string oldName{"isDisabled"}; @@ -79,11 +79,54 @@ void Controller::updateFromXMLNode(SimTK::Xml::Element& node, elem.setValue(SimTK::String(!isDisabled)); } } + if (versionNumber < 40600) { + if (node.hasElement("actuator_list")) { + // Rename element from 'actuator_list' to 'socket_actuators'. + auto actuators = node.getRequiredElement("actuator_list"); + actuators.setElementTag("socket_actuators"); + + // Store the space-delimited actuator names in a temporary + // variable. We'll use these names to connect the actuators + // to the socket in extendConnectToModel(). + std::string values = actuators.getValueAs(); + std::istringstream iss(values); + _actuatorNamesFromXML = std::vector{ + std::istream_iterator{iss}, + std::istream_iterator{}}; + + // Clear the value of the element so finalizeConnections() does + // not try to connect to invalid connectee paths. + actuators.setValueAs(""); + } + } } Super::updateFromXMLNode(node, versionNumber); } +void Controller::extendConnectToModel(Model& model) { + Super::extendConnectToModel(model); + + // If XML deserialization saved a list of actuator names, use them to + // create valid connections to the 'actuators' list Socket. + if (!_actuatorNamesFromXML.empty()) { + auto& socket = updSocket("actuators"); + for (const auto& actuatorName : _actuatorNamesFromXML) { + for (const auto& actuator : model.getComponentList()) { + if (actuator.getName() == actuatorName) { + // Connect the actuator to the socket. + addActuator(actuator); + break; + } + } + } + // Call finalizeConnection() to sync the connectee path names with the + // connected Actuators. + socket.finalizeConnection(model); + _actuatorNamesFromXML.clear(); + } +} + //============================================================================= // GET AND SET //============================================================================= diff --git a/OpenSim/Simulation/Control/Controller.h b/OpenSim/Simulation/Control/Controller.h index 23f419f81f..59871b980c 100644 --- a/OpenSim/Simulation/Control/Controller.h +++ b/OpenSim/Simulation/Control/Controller.h @@ -160,8 +160,10 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Controller, ModelComponent); * actuators. */ void setNumControls(int numControls) { _numControls = numControls; } + // MODEL COMPONENT INTERFACE void updateFromXMLNode(SimTK::Xml::Element& node, int versionNumber) override; + void extendConnectToModel(Model& model) override; private: // The number of controls this controller computes. @@ -170,6 +172,13 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Controller, ModelComponent); // Construct and initialize properties. void constructProperties(); + // Used to temporarily store actuator names when reading from XML prior to + // XMLDocument version 40600. This is used to support backwards + // compatibility with models that use the old actuator_list property. The + // actuator names are used to find actuators in the model and connect them + // to the list Socket. + std::vector _actuatorNamesFromXML; + }; // class Controller } // namespace OpenSim diff --git a/OpenSim/Simulation/Test/testInitState.cpp b/OpenSim/Simulation/Test/testInitState.cpp index 5a5aefd364..9cf119d7dc 100644 --- a/OpenSim/Simulation/Test/testInitState.cpp +++ b/OpenSim/Simulation/Test/testInitState.cpp @@ -71,6 +71,7 @@ void testStates(const string& modelFile) Model model(modelFile); ControlSetController* controller = new ControlSetController(); controller->setControlSetFileName("arm26_StaticOptimization_controls.xml"); + controller->setActuators(model.getComponentList()); model.addController( controller ); // original default state diff --git a/OpenSim/Tools/Test/testControllers.cpp b/OpenSim/Tools/Test/testControllers.cpp index e84869cc94..ffa122eb22 100644 --- a/OpenSim/Tools/Test/testControllers.cpp +++ b/OpenSim/Tools/Test/testControllers.cpp @@ -35,44 +35,94 @@ //============================================================================= #include + +#include #include using namespace OpenSim; using namespace std; -void testControlSetControllerOnBlock(); -void testPrescribedControllerOnBlock(bool enabled); -void testCorrectionControllerOnBlock(); -void testPrescribedControllerFromFile(const std::string& modelFile, - const std::string& actuatorsFile, - const std::string& controlsFile); - -int main() -{ - try { - log_info("Testing ControlSetController"); - testControlSetControllerOnBlock(); - log_info("Testing PrescribedController"); - testPrescribedControllerOnBlock(true); - testPrescribedControllerOnBlock(false); - log_info("Testing CorrectionController"); - testCorrectionControllerOnBlock(); - log_info("Testing PrescribedController from File"); - testPrescribedControllerFromFile("arm26.osim", "arm26_Reserve_Actuators.xml", - "arm26_controls.xml"); - } - catch (const std::exception& e) { - log_error("TestControllers failed due to the following error(s): {}", - e.what()); - return 1; - } - log_info("TestControllers passed."); - return 0; +namespace { + + class TestController : public Controller { + OpenSim_DECLARE_CONCRETE_OBJECT(TestController, Controller); + public: + void computeControls(const SimTK::State& s, + SimTK::Vector& controls) const override {} + + void setActuatorPath(const std::string& path) { + m_actuatorPath = path; + } + + const std::string& getActuatorPath() const { + return m_actuatorPath; + } + + protected: + void extendConnectToModel(Model& model) override { + const auto& socket = getSocket("actuators"); + const auto& actuPath = getActuatorPath(); + int index = socket.getConnecteePathIndex(actuPath); + log_cout("extendConnectToModel: connectee path index = {}", index); + + log_cout("extendConnectToModel: num connectees = {}", socket.getNumConnectees()); + } + + void extendAddToSystem(SimTK::MultibodySystem& system) const override { + const auto& socket = getSocket("actuators"); + const auto& actuPath = getActuatorPath(); + int index = socket.getConnecteePathIndex(actuPath); + log_cout("extendAddToSystem: connectee path index = {}", index); + } + + void extendFinalizeFromProperties() override { + + const auto& socket = getSocket("actuators"); + const auto& actuPath = getActuatorPath(); + int index = socket.getConnecteePathIndex(actuPath); + log_cout("extendAddToSystem: connectee path index = {}", index); + + } + + private: + std::string m_actuatorPath; + + }; + } -//========================================================================================================== -void testControlSetControllerOnBlock() -{ + + + +TEST_CASE("Test Controller interface") { + Model model = ModelFactory::createNLinkPendulum(5); + + auto* controller = new TestController(); + controller->setActuatorPath("/tau3"); + //controller->setActuators(model.getComponentList()); + + const auto& actu0 = model.getComponent("/tau0"); + const auto& actu1 = model.getComponent("/tau1"); + const auto& actu2 = model.getComponent("/tau2"); + const auto& actu3 = model.getComponent("/tau3"); + const auto& actu4 = model.getComponent("/tau4"); + + SimTK::Array_> actuators(5); + actuators[0] = &actu0; + actuators[1] = &actu1; + actuators[2] = &actu2; + actuators[3] = &actu3; + actuators[4] = &actu4; + controller->setActuators(actuators); + + model.addController(controller); + + model.initSystem(); + +} + + +TEST_CASE("testControlSetControllerOnBlock") { using namespace SimTK; // Create a new OpenSim model @@ -165,14 +215,14 @@ void testControlSetControllerOnBlock() states.print("block_push.sto"); osimModel.disownAllComponents(); -}// end of testControlSetControllerOnBlock() +} -//========================================================================================================== -void testPrescribedControllerOnBlock(bool enabled) -{ +TEST_CASE("testPrescribedControllerOnBlock") { using namespace SimTK; + auto enabled = GENERATE(true, false); + // Create a new OpenSim model Model osimModel; osimModel.setName("osimModel"); @@ -263,12 +313,10 @@ void testPrescribedControllerOnBlock(bool enabled) Storage states(manager.getStateStorage()); states.print("block_push.sto"); -}// end of testPrescribedControllerOnBlock() +} -//========================================================================================================== -void testCorrectionControllerOnBlock() -{ +TEST_CASE("testCorrectionControllerOnBlock") { using namespace SimTK; // Create a new OpenSim model @@ -315,15 +363,16 @@ void testCorrectionControllerOnBlock() manager.setIntegratorAccuracy(1.0e-4); osimModel.disownAllComponents(); -}// end of testCorrectionControllerOnBlock() +} -void testPrescribedControllerFromFile(const std::string& modelFile, - const std::string& actuatorsFile, - const std::string& controlsFile) -{ +TEST_CASE("testPrescribedControllerFromFile") { using namespace SimTK; + std::string modelFile = "arm26.osim"; + std::string actuatorsFile = "arm26_Reserve_Actuators.xml"; + std::string controlsFile = "arm26_controls.xml"; + double initialTime = 0.03; double finalTime = 1.0; From f81b27939718310ad22aa9acf042a8cf5dd998a7 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 19 Jan 2024 16:10:44 -0800 Subject: [PATCH 23/48] Add Controller interface tests --- OpenSim/Sandbox/Moco/sandboxSandbox.cpp | 20 ++--- OpenSim/Tools/Test/testControllers.cpp | 99 +++++++------------------ 2 files changed, 37 insertions(+), 82 deletions(-) diff --git a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp index 8de77ca7ce..be03e6cadd 100644 --- a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp +++ b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp @@ -24,14 +24,14 @@ using namespace OpenSim; int main() { - // TODO Logger::setLevel(Logger::Level::Debug); - MocoTrack track; - ModelProcessor modelProcessor("DeMers_mod_noarms_welds_4.0.osim"); - modelProcessor.append(ModOpReplaceMusclesWithDeGrooteFregly2016()); - modelProcessor.append(ModOpIgnoreTendonCompliance()); - track.setModel(modelProcessor); - track.setStatesReference({"r_SLD_mean_coords.sto"}); - track.set_allow_unused_references(true); - MocoSolution solution = track.solve(); - return EXIT_SUCCESS; + // TODO Logger::setLevel(Logger::Level::Debug); + MocoTrack track; + ModelProcessor modelProcessor("DeMers_mod_noarms_welds_4.0.osim"); + modelProcessor.append(ModOpReplaceMusclesWithDeGrooteFregly2016()); + modelProcessor.append(ModOpIgnoreTendonCompliance()); + track.setModel(modelProcessor); + track.setStatesReference({"r_SLD_mean_coords.sto"}); + track.set_allow_unused_references(true); + MocoSolution solution = track.solve(); + return EXIT_SUCCESS; } \ No newline at end of file diff --git a/OpenSim/Tools/Test/testControllers.cpp b/OpenSim/Tools/Test/testControllers.cpp index ffa122eb22..4b67d723ec 100644 --- a/OpenSim/Tools/Test/testControllers.cpp +++ b/OpenSim/Tools/Test/testControllers.cpp @@ -42,86 +42,41 @@ using namespace OpenSim; using namespace std; -namespace { - - class TestController : public Controller { - OpenSim_DECLARE_CONCRETE_OBJECT(TestController, Controller); - public: - void computeControls(const SimTK::State& s, - SimTK::Vector& controls) const override {} - - void setActuatorPath(const std::string& path) { - m_actuatorPath = path; - } - - const std::string& getActuatorPath() const { - return m_actuatorPath; - } - - protected: - void extendConnectToModel(Model& model) override { - const auto& socket = getSocket("actuators"); - const auto& actuPath = getActuatorPath(); - int index = socket.getConnecteePathIndex(actuPath); - log_cout("extendConnectToModel: connectee path index = {}", index); - - log_cout("extendConnectToModel: num connectees = {}", socket.getNumConnectees()); - } - - void extendAddToSystem(SimTK::MultibodySystem& system) const override { - const auto& socket = getSocket("actuators"); - const auto& actuPath = getActuatorPath(); - int index = socket.getConnecteePathIndex(actuPath); - log_cout("extendAddToSystem: connectee path index = {}", index); - } - - void extendFinalizeFromProperties() override { - - const auto& socket = getSocket("actuators"); - const auto& actuPath = getActuatorPath(); - int index = socket.getConnecteePathIndex(actuPath); - log_cout("extendAddToSystem: connectee path index = {}", index); - - } - - private: - std::string m_actuatorPath; - - }; - -} - - - - TEST_CASE("Test Controller interface") { - Model model = ModelFactory::createNLinkPendulum(5); - - auto* controller = new TestController(); - controller->setActuatorPath("/tau3"); - //controller->setActuators(model.getComponentList()); - const auto& actu0 = model.getComponent("/tau0"); - const auto& actu1 = model.getComponent("/tau1"); - const auto& actu2 = model.getComponent("/tau2"); - const auto& actu3 = model.getComponent("/tau3"); - const auto& actu4 = model.getComponent("/tau4"); + Model model = ModelFactory::createNLinkPendulum(2); + auto* controller = new PrescribedController(); + controller->prescribeControlForActuator("/tau0", new Constant(1.0)); + controller->prescribeControlForActuator("/tau1", new Constant(2.0)); - SimTK::Array_> actuators(5); - actuators[0] = &actu0; - actuators[1] = &actu1; - actuators[2] = &actu2; - actuators[3] = &actu3; - actuators[4] = &actu4; - controller->setActuators(actuators); + SECTION("No actuators connecteed") { + model.addController(controller); + REQUIRE_THROWS(model.finalizeConnections()); + } - model.addController(controller); + SECTION("Adding actuators individually") { + controller->addActuator(model.getComponent("/tau0")); + controller->addActuator(model.getComponent("/tau1")); + model.addController(controller); + REQUIRE_NOTHROW(model.finalizeConnections()); + } - model.initSystem(); + SECTION("Adding multiple actuators from a ComponentList") { + controller->setActuators(model.getComponentList()); + model.addController(controller); + REQUIRE_NOTHROW(model.finalizeConnections()); + } + SECTION("Adding multiple actuators from ReferencePtrs") { + SimTK::Array_> actuators(2); + actuators[0] = &model.getComponent("/tau0"); + actuators[1] = &model.getComponent("/tau1"); + controller->setActuators(actuators); + model.addController(controller); + REQUIRE_NOTHROW(model.finalizeConnections()); + } } - TEST_CASE("testControlSetControllerOnBlock") { using namespace SimTK; From 609ce25f8a1f8fe607fdb443b07b3952c3cf6d99 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 19 Jan 2024 16:20:00 -0800 Subject: [PATCH 24/48] Minor formatting updates --- OpenSim/Common/ComponentSocket.h | 12 ------------ OpenSim/Sandbox/Moco/sandboxSandbox.cpp | 2 +- OpenSim/Simulation/Control/ControlSetController.cpp | 1 - OpenSim/Simulation/Control/ControlSetController.h | 4 ++-- OpenSim/Tools/ActuatorForceTargetFast.cpp | 2 +- OpenSim/Tools/CMC.cpp | 2 +- 6 files changed, 5 insertions(+), 18 deletions(-) diff --git a/OpenSim/Common/ComponentSocket.h b/OpenSim/Common/ComponentSocket.h index ca4d69a182..dd5b20c8cb 100644 --- a/OpenSim/Common/ComponentSocket.h +++ b/OpenSim/Common/ComponentSocket.h @@ -253,18 +253,6 @@ class OSIMCOMMON_API AbstractSocket { return getConnecteePathProp().getValue().empty(); } - /** Check if this socket has a connectee path that matches the provided - * path. */ - bool hasConnecteePath(const std::string& path) const { - return getConnecteePathProp().findIndex(path) != -1; - } - - /** Get the index of the provided connectee path in the connectee path - * property. */ - int getConnecteePathIndex(const std::string& path) const { - return getConnecteePathProp().findIndex(path); - } - /** Get owner component of this socket */ const Component& getOwner() const { return _owner.getRef(); } diff --git a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp index be03e6cadd..d935990b59 100644 --- a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp +++ b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp @@ -34,4 +34,4 @@ int main() { track.set_allow_unused_references(true); MocoSolution solution = track.solve(); return EXIT_SUCCESS; -} \ No newline at end of file +} diff --git a/OpenSim/Simulation/Control/ControlSetController.cpp b/OpenSim/Simulation/Control/ControlSetController.cpp index d5f5349f7c..47d6712e73 100644 --- a/OpenSim/Simulation/Control/ControlSetController.cpp +++ b/OpenSim/Simulation/Control/ControlSetController.cpp @@ -30,7 +30,6 @@ // INCLUDES //============================================================================= #include "ControlSetController.h" - #include "ControlLinear.h" #include "ControlSet.h" #include "Controller.h" diff --git a/OpenSim/Simulation/Control/ControlSetController.h b/OpenSim/Simulation/Control/ControlSetController.h index 94bf35cd74..365ea83639 100644 --- a/OpenSim/Simulation/Control/ControlSetController.h +++ b/OpenSim/Simulation/Control/ControlSetController.h @@ -108,10 +108,10 @@ OpenSim_DECLARE_CONCRETE_OBJECT(ControlSetController, Controller); */ void copyData(const ControlSetController &aController); - /// read in ControlSet + /// Read in the ControlSet. void extendFinalizeFromProperties() override; - /// update the Controller's actuator list + /// Update the Controller's actuator list. void extendConnectToModel(Model& model) override; //-------------------------------------------------------------------------- diff --git a/OpenSim/Tools/ActuatorForceTargetFast.cpp b/OpenSim/Tools/ActuatorForceTargetFast.cpp index 7d84426e7c..d65a4dfeb8 100644 --- a/OpenSim/Tools/ActuatorForceTargetFast.cpp +++ b/OpenSim/Tools/ActuatorForceTargetFast.cpp @@ -132,7 +132,7 @@ prepareToOptimize(SimTK::State& s, double *x) computeConstraintVector(s, f, _constraintVector); - for(int j=0; j("actuators"); - for(int i=0; i < getNumActuators(); ++i ) { + for (int i = 0; i < getNumActuators(); ++i) { auto* act = dynamic_cast(&socket.getConnectee(i)); //Actuator& act = getActuatorSet().get(i); From 0107fcbae63be9447481fe3f5fd8a5d92916bea0 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 19 Jan 2024 16:29:45 -0800 Subject: [PATCH 25/48] Fix mixed type comparisons for failing Linux builds --- OpenSim/Simulation/Control/ControlSetController.cpp | 2 +- OpenSim/Simulation/Control/Controller.cpp | 2 +- OpenSim/Simulation/Control/PrescribedController.cpp | 5 +++-- 3 files changed, 5 insertions(+), 4 deletions(-) diff --git a/OpenSim/Simulation/Control/ControlSetController.cpp b/OpenSim/Simulation/Control/ControlSetController.cpp index 47d6712e73..9cf39a6309 100644 --- a/OpenSim/Simulation/Control/ControlSetController.cpp +++ b/OpenSim/Simulation/Control/ControlSetController.cpp @@ -277,7 +277,7 @@ void ControlSetController::extendConnectToModel(Model& model) { // Check that the actuator is connected to the controller. bool isConnected = false; - for (int iactu = 0; iactu < socket.getNumConnectees(); ++iactu) { + for (int iactu = 0; iactu < (int)socket.getNumConnectees(); ++iactu) { if (socket.getConnectee(iactu).getName() == actName) { isConnected = true; break; diff --git a/OpenSim/Simulation/Control/Controller.cpp b/OpenSim/Simulation/Control/Controller.cpp index 2afbf691d2..bfb6931e49 100644 --- a/OpenSim/Simulation/Control/Controller.cpp +++ b/OpenSim/Simulation/Control/Controller.cpp @@ -169,7 +169,7 @@ Controller::getActuators() const { const auto& socket = getSocket("actuators"); int nc = static_cast(socket.getNumConnectees()); SimTK::Array_> actuators(nc); - for (int i = 0; i < socket.getNumConnectees(); ++i) { + for (int i = 0; i < (int)socket.getNumConnectees(); ++i) { const Actuator& actu = socket.getConnectee(i); actuators[i] = &actu; } diff --git a/OpenSim/Simulation/Control/PrescribedController.cpp b/OpenSim/Simulation/Control/PrescribedController.cpp index 9e3b12505f..f2d89e111d 100644 --- a/OpenSim/Simulation/Control/PrescribedController.cpp +++ b/OpenSim/Simulation/Control/PrescribedController.cpp @@ -145,7 +145,8 @@ void PrescribedController::extendConnectToModel(Model& model) // Verify that all actuators have a control function. const FunctionSet& controlFuncs = get_ControlFunctions(); - OPENSIM_THROW_IF_FRMOBJ(controlFuncs.getSize() != socket.getNumConnectees(), + OPENSIM_THROW_IF_FRMOBJ( + controlFuncs.getSize() != (int)socket.getNumConnectees(), Exception, "The number of control functions ({}) does not match the " "number of actuators ({}) connected to the controller.", controlFuncs.getSize(), socket.getNumConnectees()); @@ -209,7 +210,7 @@ Function* PrescribedController::createFunctionFromData(const std::string& name, int PrescribedController::getActuatorIndexFromLabel( const std::string& actuLabel) const { const auto& socket = getSocket("actuators"); - for (int i = 0; i < socket.getNumConnectees(); ++i) { + for (int i = 0; i < (int)socket.getNumConnectees(); ++i) { const Actuator& actu = socket.getConnectee(i); // Check the actuator name. From 8d40b1565167a79c7dab5c322f20c8ae0bbec720 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 19 Jan 2024 17:23:16 -0800 Subject: [PATCH 26/48] Support setting all actuators with 'ALL' flag --- OpenSim/Simulation/Control/Controller.cpp | 7 ++++ .../Control/PrescribedController.cpp | 35 +++++++++++++++---- 2 files changed, 35 insertions(+), 7 deletions(-) diff --git a/OpenSim/Simulation/Control/Controller.cpp b/OpenSim/Simulation/Control/Controller.cpp index bfb6931e49..9bff4f6505 100644 --- a/OpenSim/Simulation/Control/Controller.cpp +++ b/OpenSim/Simulation/Control/Controller.cpp @@ -112,6 +112,13 @@ void Controller::extendConnectToModel(Model& model) { if (!_actuatorNamesFromXML.empty()) { auto& socket = updSocket("actuators"); for (const auto& actuatorName : _actuatorNamesFromXML) { + // If the actuator name is "ALL", connect all actuators and break. + if (IO::Uppercase(actuatorName) == "ALL") { + setActuators(model.getComponentList()); + break; + } + + // Otherwise, find the actuator by name and connect it. for (const auto& actuator : model.getComponentList()) { if (actuator.getName() == actuatorName) { // Connect the actuator to the socket. diff --git a/OpenSim/Simulation/Control/PrescribedController.cpp b/OpenSim/Simulation/Control/PrescribedController.cpp index f2d89e111d..84d72d9e65 100644 --- a/OpenSim/Simulation/Control/PrescribedController.cpp +++ b/OpenSim/Simulation/Control/PrescribedController.cpp @@ -69,7 +69,7 @@ void PrescribedController::constructProperties() void PrescribedController::extendConnectToModel(Model& model) { Super::extendConnectToModel(model); - const auto& socket = getSocket("actuators"); + auto& socket = updSocket("actuators"); // If a controls file was specified, load it and create control functions // for any actuators that do not already have one. @@ -99,12 +99,33 @@ void PrescribedController::extendConnectToModel(Model& model) const string& columnLabel = columnLabels[i]; if (!_actuLabelsToControlFunctionIndexMap.count(columnLabel)) { - // Search for the column label in the model's actuators. + // See if the column label matches a connected actuator. If not, + // find and add the actuator to the controller. int actuIndex = getActuatorIndexFromLabel(columnLabel); - OPENSIM_THROW_IF_FRMOBJ(actuIndex < 0, Exception, - "The controls file contains column {}, but no Actuator " - "with this label is connected to the controller.", - columnLabel); + if (actuIndex < 0) { + bool foundActuator = false; + for (const auto& actu : model.getComponentList()) { + if (actu.getName() == columnLabel) { + addActuator(actu); + foundActuator = true; + break; + } + if (actu.getAbsolutePathString() == columnLabel) { + addActuator(actu); + foundActuator = true; + break; + } + } + OPENSIM_THROW_IF_FRMOBJ(!foundActuator, Exception, + "Control provided from file with label {}, but no " + "matching Actuator was found in the model.", + columnLabel) + + // If we found a matching actuator, call + // finalizeConnection() to sync the connectee path names + // with the Actuator connectee. + socket.finalizeConnection(model); + } // Create the control function and assign it to the actuator. controls.getDataColumn( @@ -161,7 +182,7 @@ void PrescribedController::computeControls(const SimTK::State& s, SimTK::Vector time(1, s.getTime()); const auto& socket = getSocket("actuators"); - for(int i = 0; i < socket.getNumConnectees(); ++i){ + for(int i = 0; i < (int)socket.getNumConnectees(); ++i){ actControls[0] = get_ControlFunctions()[i].calcValue(time); socket.getConnectee(i).addInControls(actControls, controls); } From e22930bdb9dddd3da38d0ec4a8d88980e59ec1e4 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 19 Jan 2024 23:13:07 -0800 Subject: [PATCH 27/48] Attempt to fix CMC tests --- OpenSim/Tools/CMCTool.cpp | 57 +++++++++++++++++++++++++++++++-------- OpenSim/Tools/CMCTool.h | 8 +++--- 2 files changed, 50 insertions(+), 15 deletions(-) diff --git a/OpenSim/Tools/CMCTool.cpp b/OpenSim/Tools/CMCTool.cpp index f99a38ab76..7c438a6b02 100644 --- a/OpenSim/Tools/CMCTool.cpp +++ b/OpenSim/Tools/CMCTool.cpp @@ -408,15 +408,29 @@ bool CMCTool::run() //taskSet.print("cmcTasksRT.xml"); log_info("TaskSet size = {}.", taskSet.getSize()); - CMC* controller = new CMC(_model,&taskSet); // Need to make it a pointer since Model takes ownership + CMC* controller = new CMC(_model,&taskSet); // Need to make it a pointer since Model takes ownership controller->setName( "CMC" ); // Don't automatically give CMC all the model actuators // Check to see if user has explicitly listed Actuators to be // excluded from CMC's control. - - controller->setActuators(getActuatorsForCMC(_excludedActuators)); - _model->addController(controller ); + std::vector actuatorPaths = + getActuatorsForCMC(_excludedActuators); + for (const auto& actuatorPath : actuatorPaths) { + if (_model->hasComponent(actuatorPath) && !actuatorPath.empty() + && actuatorPath != ComponentPath::root()) { + const auto& actuator = _model->getComponent(actuatorPath); + controller->addActuator(actuator); + } else { + for (const auto& actu : _model->getComponentList()) { + if (actu.getName() == actuatorPath) { + controller->addActuator(actu); + break; + } + } + } + } + _model->addController(controller); controller->setEnabled(true); controller->setUseCurvatureFilter(false); controller->setTargetDT(_targetDT); @@ -1089,7 +1103,7 @@ void CMCTool::setOriginalForceSet(const ForceSet &aForceSet) { /* Get the Set of model actuators for CMC that exclude user specified Actuators */ -Array_> CMCTool::getActuatorsForCMC( +std::vector CMCTool::getActuatorsForCMC( const Array& actuatorsByNameOrGroup) { const Set& actuatorsForCMC = _model->getActuators(); @@ -1103,7 +1117,8 @@ Array_> CMCTool::getActuatorsForCMC( Array groupNames; actuatorsForCMC.getGroupNames(groupNames); - /* The search for individual group or force names IS case-sensitive BUT keywords are not*/ + // The search for individual group or force names IS case-sensitive, but + // keywords are not. for (int i = 0; i < actuatorsByNameOrGroup.getSize(); i++) { // index result when a name is not a force or group name for forces in // the model @@ -1138,16 +1153,36 @@ Array_> CMCTool::getActuatorsForCMC( } const int numActu = actuatorsForCMC.getSize(); - Array_> actuatorsForCMCRefs(numActu); + std::vector actuatorsForCMCPaths(numActu); for (int i = 0; i < numActu; ++i) { + // Skip actuators that are not force-generating. Check both the path + // and name of the actuator. const auto& actuPath = actuatorsForCMC.get(i).getAbsolutePathString(); + const auto& actuName = actuatorsForCMC.get(i).getName(); if (std::find(actuatorsToSkip.begin(), actuatorsToSkip.end(), actuPath) != - actuatorsToSkip.end()) { + actuatorsToSkip.end()) { continue; } - const auto& actuator = _model->getComponent(actuPath); - actuatorsForCMCRefs[i] = &actuator; + if (std::find(actuatorsToSkip.begin(), actuatorsToSkip.end(), actuName) != + actuatorsToSkip.end()) { + continue; + } + + // If the actuator has a valid path, use it to retrieve the actuator. + // Otherwise, use the name to search for the actuator. + if (_model->hasComponent(actuPath) && !actuPath.empty() + && actuPath != ComponentPath::root()) { + const auto& actuator = _model->getComponent(actuPath); + actuatorsForCMCPaths.push_back(actuator.getAbsolutePathString()); + } else { + for (const auto& actu : _model->getComponentList()) { + if (actu.getName() == actuName) { + actuatorsForCMCPaths.push_back(actuName); + break; + } + } + } } - return actuatorsForCMCRefs; + return actuatorsForCMCPaths; } diff --git a/OpenSim/Tools/CMCTool.h b/OpenSim/Tools/CMCTool.h index c38051ff05..5f5e523a8f 100644 --- a/OpenSim/Tools/CMCTool.h +++ b/OpenSim/Tools/CMCTool.h @@ -141,10 +141,10 @@ OpenSim_DECLARE_CONCRETE_OBJECT(CMCTool, AbstractTool); void setNull(); void setupProperties(); - /** Get the array of model actuators for CMC that exclude user specified - Actuators */ - SimTK::Array_> - getActuatorsForCMC(const Array &actuatorsByNameOrGroup); + /** Get the vector of model actuator paths for CMC that exclude user + specified Actuators */ + std::vector getActuatorsForCMC( + const Array &actuatorsByNameOrGroup); //-------------------------------------------------------------------------- // OPERATORS From 70e51f0c0ddf499d52c4580daa3014e255087638 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Sun, 21 Jan 2024 14:37:10 -0800 Subject: [PATCH 28/48] Continue fixing failing tests --- Applications/Analyze/test/testAnalyzeTool.cpp | 16 ++++-------- .../tests/test_swig_additional_interface.py | 13 +++++++--- OpenSim/Common/Component.h | 2 ++ .../Control/ControlSetController.cpp | 25 ++++++++++++++++--- OpenSim/Tools/ForwardTool.cpp | 8 +++++- 5 files changed, 45 insertions(+), 19 deletions(-) diff --git a/Applications/Analyze/test/testAnalyzeTool.cpp b/Applications/Analyze/test/testAnalyzeTool.cpp index 5f9629edf7..79ded35a25 100644 --- a/Applications/Analyze/test/testAnalyzeTool.cpp +++ b/Applications/Analyze/test/testAnalyzeTool.cpp @@ -489,19 +489,13 @@ void testIMUDataReporter() { pendulum.getComponent("/tau0")); controller->addActuator( pendulum.getComponent("/tau1")); - pendulum.addController(controller); - pendulum.initSystem(); - - // Specify constant torque functions to the torque acutators via the - // PrescribedController we added previously. + controller->prescribeControlForActuator("tau0", new Constant(0.0)); + // Specify constant torque functions to the torque actuators Constant* constantTorque0 = new Constant(10.0); Constant* constantTorque1 = new Constant(10.0); - pendulum.updComponent( - "/controllerset/torque_controller") - .prescribeControlForActuator("tau0", constantTorque0); - pendulum.updComponent( - "/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. diff --git a/Bindings/Python/tests/test_swig_additional_interface.py b/Bindings/Python/tests/test_swig_additional_interface.py index cb301d283e..70d24cb7a5 100644 --- a/Bindings/Python/tests/test_swig_additional_interface.py +++ b/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() diff --git a/OpenSim/Common/Component.h b/OpenSim/Common/Component.h index 355e352ca4..4744cd1fff 100644 --- a/OpenSim/Common/Component.h +++ b/OpenSim/Common/Component.h @@ -3506,6 +3506,8 @@ void Socket::finalizeConnection(const Component& root) { const auto& comp = *connectee; const auto& rootOfConnectee = comp.getRoot(); const auto& myRoot = getOwner().getRoot(); + std::cout << "rootOfConnectee: " << rootOfConnectee.getName() << std::endl; + std::cout << "myRoot: " << myRoot.getName() << std::endl; if (&myRoot != &rootOfConnectee) { std::stringstream msg; msg << "Socket<" << getConnecteeTypeName() << "> '" << getName() diff --git a/OpenSim/Simulation/Control/ControlSetController.cpp b/OpenSim/Simulation/Control/ControlSetController.cpp index 9cf39a6309..c59f31161b 100644 --- a/OpenSim/Simulation/Control/ControlSetController.cpp +++ b/OpenSim/Simulation/Control/ControlSetController.cpp @@ -269,6 +269,7 @@ void ControlSetController::extendConnectToModel(Model& model) { std::string ext = ".excitation"; for (int i = 0; _controlSet != nullptr && i < _controlSet->getSize(); ++i) { std::string actName = _controlSet->get(i).getName(); + std::cout << "actName = " << actName << std::endl; if (actName.length() > ext.length() && !actName.compare( actName.length() - ext.length(), ext.length(), ext)) { @@ -279,15 +280,33 @@ void ControlSetController::extendConnectToModel(Model& model) { bool isConnected = false; for (int iactu = 0; iactu < (int)socket.getNumConnectees(); ++iactu) { if (socket.getConnectee(iactu).getName() == actName) { + log_cout("ControlSetController::extendConnectToModel " + "Actuator '{}' already connected to ControlSetController '{}'.", + actName, getName()); isConnected = true; break; } } + // If not already connected, try to connect to an actuator in the model. + if (!isConnected) { + for (const auto& actu : model.getComponentList()) { + if (actu.getName() == actName) { + log_cout("ControlSetController::extendConnectToModel " + "Connecting ControlSetController '{}' to Actuator" + "'{}'.", getName(), actu.getName()); + addActuator(actu); + updSocket("actuators").finalizeConnection(model); + isConnected = true; + break; + } + } + } + OPENSIM_THROW_IF_FRMOBJ(!isConnected, Exception, - "ControlSetController::extendFinalizeFromProperties: " - "Actuator '{}' in ControlSet '{}' not connected to controller " - " '{}'.", actName, _controlSet->getName(), getName()); + "Control with name '{}' provided in the ControlSet '{}', but no " + "matching Actuator was found in the model.", + actName, _controlSet->getName()); } } diff --git a/OpenSim/Tools/ForwardTool.cpp b/OpenSim/Tools/ForwardTool.cpp index 79216b38c1..83ff5c7c40 100644 --- a/OpenSim/Tools/ForwardTool.cpp +++ b/OpenSim/Tools/ForwardTool.cpp @@ -244,7 +244,13 @@ bool ForwardTool::run() // so that the parsing code behaves properly if called from a different directory. auto cwd = IO::CwdChanger::changeToParentOf(getDocumentFileName()); - /*bool externalLoads = */createExternalLoads(_externalLoadsFileName, *_model); + ///*bool externalLoads = */createExternalLoads(_externalLoadsFileName, *_model); + if (_externalLoadsFileName.empty()) { + log_warn("External loads file '{}' is ignored.", _externalLoadsFileName); + } else { + auto* externalLoads = new ExternalLoads(_externalLoadsFileName, true); + _model->addModelComponent(externalLoads); + } // Re create the system with forces above and Realize the topology SimTK::State& s = _model->initSystem(); From eaec8dd547e426177a88605a41293b8ab5c3e49a Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 22 Jan 2024 09:35:04 -0800 Subject: [PATCH 29/48] Fixing failing test pt.2 --- Applications/Forward/test/testForward.cpp | 356 +++++++++--------- Bindings/Java/tests/TestModelBuilding.java | 4 +- .../tests/test_swig_additional_interface.py | 2 +- OpenSim/Common/Component.h | 4 +- .../Control/ControlSetController.cpp | 52 +-- OpenSim/Simulation/Model/AbstractTool.cpp | 14 + OpenSim/Tools/ForwardTool.cpp | 15 +- 7 files changed, 232 insertions(+), 215 deletions(-) diff --git a/Applications/Forward/test/testForward.cpp b/Applications/Forward/test/testForward.cpp index 257e9a3f67..217d9a8f9e 100644 --- a/Applications/Forward/test/testForward.cpp +++ b/Applications/Forward/test/testForward.cpp @@ -33,12 +33,12 @@ using namespace OpenSim; using namespace std; -void testPendulum(); -void testPendulumExternalLoad(); -void testPendulumExternalLoadWithPointInGround(); -void testArm26(); -void testGait2354(); -void testGait2354WithController(); +//void testPendulum(); +//void testPendulumExternalLoad(); +//void testPendulumExternalLoadWithPointInGround(); +//void testArm26(); +//void testGait2354(); +//void testGait2354WithController(); void testGait2354WithControllerGUI(); @@ -47,183 +47,183 @@ int main() { 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); +// 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() { - ForwardTool forward("pendulum_Setup_Forward.xml"); - forward.run(); - Storage storage("Results/pendulum_states.sto"); - ASSERT(storage.getFirstTime() == 0.0); - ASSERT(storage.getLastTime() == 1.0); - - // Since the pendulum is only swinging through small angles, it should be very - // close to a simple harmonic oscillator. - double previousTime = -1.0; - double amp = SimTK::Pi/20; - double k = sqrt(9.80665000/0.5); - for (int j = 0; j < storage.getSize(); ++j) { - StateVector* state = storage.getStateVector(j); - double time = state->getTime(); - ASSERT(time > previousTime); - previousTime = time; - ASSERT_EQUAL(-amp*cos(k*time), state->getData()[0], 1.0e-2); - ASSERT_EQUAL(amp*k*sin(k*time), state->getData()[1],1.0e-2); - } - ASSERT(previousTime == 1.0); -} - - -void testPendulumExternalLoad() { - ForwardTool forward("pendulum_ext_gravity_Setup_Forward.xml"); - forward.run(); - Storage results("Results/pendulum_ext_gravity_states.sto"); - ASSERT(results.getFirstTime() == 0.0); - ASSERT(results.getLastTime() == 1.0); - - Storage standard("Results/pendulum_states.sto"); - - - Array data; - int i = results.getSize() - 1; - StateVector* state = results.getStateVector(i); - double time = state->getTime(); - data.setSize(state->getSize()); - standard.getDataAtTime(time, state->getSize(), data); - int nc = forward.getModel().getNumCoordinates(); - 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]; - ASSERT_EQUAL(data[j], state->getData()[j], 1e-2, - __FILE__, __LINE__, "ASSERT_EQUAL FAILED " + message.str()); - cout << "ASSERT_EQUAL PASSED " << message.str() << endl; - } -} - - -void testPendulumExternalLoadWithPointInGround() { - ForwardTool forward("pendulum_ext_point_in_ground_Setup_Forward.xml"); - forward.run(); - - Storage results("Results/pendulum_ext_gravity_point_in_ground_states.sto"); - ASSERT(results.getFirstTime() == 0.0); - ASSERT(results.getLastTime() == 1.0); - - Storage standard("Results/pendulum_states.sto"); - Array data; - int i = results.getSize() - 1; - StateVector* state = results.getStateVector(i); - double time = state->getTime(); - data.setSize(state->getSize()); - standard.getDataAtTime(time, state->getSize(), data); - int nc = forward.getModel().getNumCoordinates(); - 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]; - cout << message.str() << endl; - ASSERT_EQUAL(data[j], state->getData()[j], 1e-2, - __FILE__, __LINE__, "ASSERT_EQUAL FAILED " + message.str()); - cout << "ASSERT_EQUAL PASSED " << endl; - } -} - - -void testArm26() { - ForwardTool forward("arm26_Setup_Forward.xml"); - forward.run(); - - Storage results("Results/arm26_states.sto"); - Storage* standard = new Storage(); - string statesFileName("std_arm26_states.sto"); - forward.loadStatesStorage( statesFileName, standard ); - StateVector* state = results.getStateVector(0); - double time = state->getTime(); - Array data; - data.setSize(state->getSize()); - standard->getDataAtTime(time, state->getSize(), data); - 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, - __FILE__, __LINE__, "ASSERT_EQUAL FAILED " + message.str()); - cout << "ASSERT_EQUAL PASSED " << message.str(); - } - - int i = results.getSize()-1; - state = results.getStateVector(i); - time = state->getTime(); - data.setSize(state->getSize()); - standard->getDataAtTime(time, state->getSize(), data); - 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, - __FILE__, __LINE__, "ASSERT_EQUAL FAILED " + message.str()); - cout << "ASSERT_EQUAL PASSED " << message.str(); - } -} - -void testGait2354() -{ - ForwardTool forward("subject01_Setup_Forward.xml"); - forward.run(); - Storage results("Results/subject01_states.sto"); - - //Storage standard("std_subject01_walk1_states.sto"); - 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 rms_tols(2*nstates, 0.001); //activations and fiber-lengths - - for(int i=0; i rms_tols(2*nstates, 0.001); //activations and fiber-lengths - - for(int i=0; igetTime(); +// ASSERT(time > previousTime); +// previousTime = time; +// ASSERT_EQUAL(-amp*cos(k*time), state->getData()[0], 1.0e-2); +// ASSERT_EQUAL(amp*k*sin(k*time), state->getData()[1],1.0e-2); +// } +// ASSERT(previousTime == 1.0); +//} +// +// +//void testPendulumExternalLoad() { +// ForwardTool forward("pendulum_ext_gravity_Setup_Forward.xml"); +// forward.run(); +// Storage results("Results/pendulum_ext_gravity_states.sto"); +// ASSERT(results.getFirstTime() == 0.0); +// ASSERT(results.getLastTime() == 1.0); +// +// Storage standard("Results/pendulum_states.sto"); +// +// +// Array data; +// int i = results.getSize() - 1; +// StateVector* state = results.getStateVector(i); +// double time = state->getTime(); +// data.setSize(state->getSize()); +// standard.getDataAtTime(time, state->getSize(), data); +// int nc = forward.getModel().getNumCoordinates(); +// 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]; +// ASSERT_EQUAL(data[j], state->getData()[j], 1e-2, +// __FILE__, __LINE__, "ASSERT_EQUAL FAILED " + message.str()); +// cout << "ASSERT_EQUAL PASSED " << message.str() << endl; +// } +//} +// +// +//void testPendulumExternalLoadWithPointInGround() { +// ForwardTool forward("pendulum_ext_point_in_ground_Setup_Forward.xml"); +// forward.run(); +// +// Storage results("Results/pendulum_ext_gravity_point_in_ground_states.sto"); +// ASSERT(results.getFirstTime() == 0.0); +// ASSERT(results.getLastTime() == 1.0); +// +// Storage standard("Results/pendulum_states.sto"); +// Array data; +// int i = results.getSize() - 1; +// StateVector* state = results.getStateVector(i); +// double time = state->getTime(); +// data.setSize(state->getSize()); +// standard.getDataAtTime(time, state->getSize(), data); +// int nc = forward.getModel().getNumCoordinates(); +// 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]; +// cout << message.str() << endl; +// ASSERT_EQUAL(data[j], state->getData()[j], 1e-2, +// __FILE__, __LINE__, "ASSERT_EQUAL FAILED " + message.str()); +// cout << "ASSERT_EQUAL PASSED " << endl; +// } +//} +// +// +//void testArm26() { +// ForwardTool forward("arm26_Setup_Forward.xml"); +// forward.run(); +// +// Storage results("Results/arm26_states.sto"); +// Storage* standard = new Storage(); +// string statesFileName("std_arm26_states.sto"); +// forward.loadStatesStorage( statesFileName, standard ); +// StateVector* state = results.getStateVector(0); +// double time = state->getTime(); +// Array data; +// data.setSize(state->getSize()); +// standard->getDataAtTime(time, state->getSize(), data); +// 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, +// __FILE__, __LINE__, "ASSERT_EQUAL FAILED " + message.str()); +// cout << "ASSERT_EQUAL PASSED " << message.str(); +// } +// +// int i = results.getSize()-1; +// state = results.getStateVector(i); +// time = state->getTime(); +// data.setSize(state->getSize()); +// standard->getDataAtTime(time, state->getSize(), data); +// 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, +// __FILE__, __LINE__, "ASSERT_EQUAL FAILED " + message.str()); +// cout << "ASSERT_EQUAL PASSED " << message.str(); +// } +//} +// +//void testGait2354() +//{ +// ForwardTool forward("subject01_Setup_Forward.xml"); +// forward.run(); +// Storage results("Results/subject01_states.sto"); +// +// //Storage standard("std_subject01_walk1_states.sto"); +// 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 rms_tols(2*nstates, 0.001); //activations and fiber-lengths +// +// for(int i=0; i rms_tols(2*nstates, 0.001); //activations and fiber-lengths +// +// for(int i=0; i::finalizeConnection(const Component& root) { const auto& comp = *connectee; const auto& rootOfConnectee = comp.getRoot(); const auto& myRoot = getOwner().getRoot(); - std::cout << "rootOfConnectee: " << rootOfConnectee.getName() << std::endl; - std::cout << "myRoot: " << myRoot.getName() << std::endl; +// std::cout << "rootOfConnectee: " << rootOfConnectee.getName() << std::endl; +// std::cout << "myRoot: " << myRoot.getName() << std::endl; if (&myRoot != &rootOfConnectee) { std::stringstream msg; msg << "Socket<" << getConnecteeTypeName() << "> '" << getName() diff --git a/OpenSim/Simulation/Control/ControlSetController.cpp b/OpenSim/Simulation/Control/ControlSetController.cpp index c59f31161b..6f0ff859c2 100644 --- a/OpenSim/Simulation/Control/ControlSetController.cpp +++ b/OpenSim/Simulation/Control/ControlSetController.cpp @@ -277,36 +277,38 @@ void ControlSetController::extendConnectToModel(Model& model) { } // Check that the actuator is connected to the controller. - bool isConnected = false; - for (int iactu = 0; iactu < (int)socket.getNumConnectees(); ++iactu) { - if (socket.getConnectee(iactu).getName() == actName) { - log_cout("ControlSetController::extendConnectToModel " - "Actuator '{}' already connected to ControlSetController '{}'.", - actName, getName()); - isConnected = true; - break; - } - } +// bool isConnected = false; +// for (int iactu = 0; iactu < (int)socket.getNumConnectees(); ++iactu) { +// if (socket.getConnectee(iactu).getName() == actName) { +// log_cout("ControlSetController::extendConnectToModel " +// "Actuator '{}' already connected to ControlSetController '{}'.", +// actName, getName()); +// isConnected = true; +// break; +// } +// } // If not already connected, try to connect to an actuator in the model. - if (!isConnected) { - for (const auto& actu : model.getComponentList()) { - if (actu.getName() == actName) { - log_cout("ControlSetController::extendConnectToModel " - "Connecting ControlSetController '{}' to Actuator" - "'{}'.", getName(), actu.getName()); - addActuator(actu); - updSocket("actuators").finalizeConnection(model); - isConnected = true; - break; - } +// if (!isConnected) { + updSocket("actuators").disconnect(); + + for (const auto& actu : model.getComponentList()) { + if (actu.getName() == actName) { + log_cout("ControlSetController::extendConnectToModel " + "Connecting ControlSetController '{}' to Actuator" + "'{}'.", getName(), actu.getName()); + addActuator(actu); +// isConnected = true; + break; } } +// } + updSocket("actuators").finalizeConnection(model); - OPENSIM_THROW_IF_FRMOBJ(!isConnected, Exception, - "Control with name '{}' provided in the ControlSet '{}', but no " - "matching Actuator was found in the model.", - actName, _controlSet->getName()); +// OPENSIM_THROW_IF_FRMOBJ(!isConnected, Exception, +// "Control with name '{}' provided in the ControlSet '{}', but no " +// "matching Actuator was found in the model.", +// actName, _controlSet->getName()); } } diff --git a/OpenSim/Simulation/Model/AbstractTool.cpp b/OpenSim/Simulation/Model/AbstractTool.cpp index dcac598069..7e333929e0 100644 --- a/OpenSim/Simulation/Model/AbstractTool.cpp +++ b/OpenSim/Simulation/Model/AbstractTool.cpp @@ -462,8 +462,22 @@ addControllerSetToModel() int size = _controllerSet.getSize(); _controllerCopies.setMemoryOwner(false); + std::cout << "DEBUG controller set size: " << size << std::endl; for(int i=0;i("actuators").disconnect(); Controller *controller = _controllerSet.get(i).clone(); + const auto& socket = _controllerSet.get(i).getSocket("actuators"); + std::cout << "DEBUG num connectees: " << socket.getNumConnectees() << std::endl; + const auto& socketCopy = controller->updSocket("actuators"); + for (int j = 0; i < socket.getNumConnectees(); ++i) { + const auto& actuator = socket.getConnectee(j); + const auto& actuatorCopy = socketCopy.getConnectee(j); + + std::cout << "DEBUG connectee path: " << socket.getConnecteePath(j) << std::endl; + std::cout << "DEBUG connectee path copy: " << socketCopy.getConnecteePath(j) << std::endl; + + } + _model->addController(controller); _controllerCopies.adoptAndAppend(controller); } diff --git a/OpenSim/Tools/ForwardTool.cpp b/OpenSim/Tools/ForwardTool.cpp index 83ff5c7c40..70be288471 100644 --- a/OpenSim/Tools/ForwardTool.cpp +++ b/OpenSim/Tools/ForwardTool.cpp @@ -244,13 +244,14 @@ bool ForwardTool::run() // so that the parsing code behaves properly if called from a different directory. auto cwd = IO::CwdChanger::changeToParentOf(getDocumentFileName()); - ///*bool externalLoads = */createExternalLoads(_externalLoadsFileName, *_model); - if (_externalLoadsFileName.empty()) { - log_warn("External loads file '{}' is ignored.", _externalLoadsFileName); - } else { - auto* externalLoads = new ExternalLoads(_externalLoadsFileName, true); - _model->addModelComponent(externalLoads); - } + std::cout << "DEBUG: ForwardTool::run() _externalLoadsFileName = " << _externalLoadsFileName << std::endl; + /*bool externalLoads = */createExternalLoads(_externalLoadsFileName, *_model); +// if (_externalLoadsFileName.empty()) { +// log_warn("External loads file '{}' is ignored.", _externalLoadsFileName); +// } else { +// auto* externalLoads = new ExternalLoads(_externalLoadsFileName, true); +// _model->addModelComponent(externalLoads); +// } // Re create the system with forces above and Realize the topology SimTK::State& s = _model->initSystem(); From e4f664ba7c4b26ed7b278601e41d5be2d9f5f1bb Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 22 Jan 2024 11:14:11 -0800 Subject: [PATCH 30/48] Fix broken connectee root issue in testForward --- Applications/Forward/test/testForward.cpp | 359 +++++++++--------- OpenSim/Common/Component.h | 2 - .../Control/ControlSetController.cpp | 50 ++- OpenSim/Tools/ForwardTool.cpp | 6 - 4 files changed, 204 insertions(+), 213 deletions(-) diff --git a/Applications/Forward/test/testForward.cpp b/Applications/Forward/test/testForward.cpp index 217d9a8f9e..d62586995b 100644 --- a/Applications/Forward/test/testForward.cpp +++ b/Applications/Forward/test/testForward.cpp @@ -33,12 +33,12 @@ using namespace OpenSim; using namespace std; -//void testPendulum(); -//void testPendulumExternalLoad(); -//void testPendulumExternalLoadWithPointInGround(); -//void testArm26(); -//void testGait2354(); -//void testGait2354WithController(); +void testPendulum(); +void testPendulumExternalLoad(); +void testPendulumExternalLoadWithPointInGround(); +void testArm26(); +void testGait2354(); +void testGait2354WithController(); void testGait2354WithControllerGUI(); @@ -47,183 +47,184 @@ int main() { 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); + 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() { -// ForwardTool forward("pendulum_Setup_Forward.xml"); -// forward.run(); -// Storage storage("Results/pendulum_states.sto"); -// ASSERT(storage.getFirstTime() == 0.0); -// ASSERT(storage.getLastTime() == 1.0); -// -// // Since the pendulum is only swinging through small angles, it should be very -// // close to a simple harmonic oscillator. -// double previousTime = -1.0; -// double amp = SimTK::Pi/20; -// double k = sqrt(9.80665000/0.5); -// for (int j = 0; j < storage.getSize(); ++j) { -// StateVector* state = storage.getStateVector(j); -// double time = state->getTime(); -// ASSERT(time > previousTime); -// previousTime = time; -// ASSERT_EQUAL(-amp*cos(k*time), state->getData()[0], 1.0e-2); -// ASSERT_EQUAL(amp*k*sin(k*time), state->getData()[1],1.0e-2); -// } -// ASSERT(previousTime == 1.0); -//} -// -// -//void testPendulumExternalLoad() { -// ForwardTool forward("pendulum_ext_gravity_Setup_Forward.xml"); -// forward.run(); -// Storage results("Results/pendulum_ext_gravity_states.sto"); -// ASSERT(results.getFirstTime() == 0.0); -// ASSERT(results.getLastTime() == 1.0); -// -// Storage standard("Results/pendulum_states.sto"); -// -// -// Array data; -// int i = results.getSize() - 1; -// StateVector* state = results.getStateVector(i); -// double time = state->getTime(); -// data.setSize(state->getSize()); -// standard.getDataAtTime(time, state->getSize(), data); -// int nc = forward.getModel().getNumCoordinates(); -// 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]; -// ASSERT_EQUAL(data[j], state->getData()[j], 1e-2, -// __FILE__, __LINE__, "ASSERT_EQUAL FAILED " + message.str()); -// cout << "ASSERT_EQUAL PASSED " << message.str() << endl; -// } -//} -// -// -//void testPendulumExternalLoadWithPointInGround() { -// ForwardTool forward("pendulum_ext_point_in_ground_Setup_Forward.xml"); -// forward.run(); -// -// Storage results("Results/pendulum_ext_gravity_point_in_ground_states.sto"); -// ASSERT(results.getFirstTime() == 0.0); -// ASSERT(results.getLastTime() == 1.0); -// -// Storage standard("Results/pendulum_states.sto"); -// Array data; -// int i = results.getSize() - 1; -// StateVector* state = results.getStateVector(i); -// double time = state->getTime(); -// data.setSize(state->getSize()); -// standard.getDataAtTime(time, state->getSize(), data); -// int nc = forward.getModel().getNumCoordinates(); -// 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]; -// cout << message.str() << endl; -// ASSERT_EQUAL(data[j], state->getData()[j], 1e-2, -// __FILE__, __LINE__, "ASSERT_EQUAL FAILED " + message.str()); -// cout << "ASSERT_EQUAL PASSED " << endl; -// } -//} -// -// -//void testArm26() { -// ForwardTool forward("arm26_Setup_Forward.xml"); -// forward.run(); -// -// Storage results("Results/arm26_states.sto"); -// Storage* standard = new Storage(); -// string statesFileName("std_arm26_states.sto"); -// forward.loadStatesStorage( statesFileName, standard ); -// StateVector* state = results.getStateVector(0); -// double time = state->getTime(); -// Array data; -// data.setSize(state->getSize()); -// standard->getDataAtTime(time, state->getSize(), data); -// 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, -// __FILE__, __LINE__, "ASSERT_EQUAL FAILED " + message.str()); -// cout << "ASSERT_EQUAL PASSED " << message.str(); -// } -// -// int i = results.getSize()-1; -// state = results.getStateVector(i); -// time = state->getTime(); -// data.setSize(state->getSize()); -// standard->getDataAtTime(time, state->getSize(), data); -// 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, -// __FILE__, __LINE__, "ASSERT_EQUAL FAILED " + message.str()); -// cout << "ASSERT_EQUAL PASSED " << message.str(); -// } -//} -// -//void testGait2354() -//{ -// ForwardTool forward("subject01_Setup_Forward.xml"); -// forward.run(); -// Storage results("Results/subject01_states.sto"); -// -// //Storage standard("std_subject01_walk1_states.sto"); -// 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 rms_tols(2*nstates, 0.001); //activations and fiber-lengths -// -// for(int i=0; i rms_tols(2*nstates, 0.001); //activations and fiber-lengths -// -// for(int i=0; igetTime(); + ASSERT(time > previousTime); + previousTime = time; + ASSERT_EQUAL(-amp*cos(k*time), state->getData()[0], 1.0e-2); + ASSERT_EQUAL(amp*k*sin(k*time), state->getData()[1],1.0e-2); + } + ASSERT(previousTime == 1.0); +} + + +void testPendulumExternalLoad() { + ForwardTool forward("pendulum_ext_gravity_Setup_Forward.xml"); + forward.run(); + Storage results("Results/pendulum_ext_gravity_states.sto"); + ASSERT(results.getFirstTime() == 0.0); + ASSERT(results.getLastTime() == 1.0); + + Storage standard("Results/pendulum_states.sto"); + + + Array data; + int i = results.getSize() - 1; + StateVector* state = results.getStateVector(i); + double time = state->getTime(); + data.setSize(state->getSize()); + standard.getDataAtTime(time, state->getSize(), data); + int nc = forward.getModel().getNumCoordinates(); + 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]; + ASSERT_EQUAL(data[j], state->getData()[j], 1e-2, + __FILE__, __LINE__, "ASSERT_EQUAL FAILED " + message.str()); + cout << "ASSERT_EQUAL PASSED " << message.str() << endl; + } +} + + +void testPendulumExternalLoadWithPointInGround() { + ForwardTool forward("pendulum_ext_point_in_ground_Setup_Forward.xml"); + forward.run(); + + Storage results("Results/pendulum_ext_gravity_point_in_ground_states.sto"); + ASSERT(results.getFirstTime() == 0.0); + ASSERT(results.getLastTime() == 1.0); + + Storage standard("Results/pendulum_states.sto"); + Array data; + int i = results.getSize() - 1; + StateVector* state = results.getStateVector(i); + double time = state->getTime(); + data.setSize(state->getSize()); + standard.getDataAtTime(time, state->getSize(), data); + int nc = forward.getModel().getNumCoordinates(); + 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]; + cout << message.str() << endl; + ASSERT_EQUAL(data[j], state->getData()[j], 1e-2, + __FILE__, __LINE__, "ASSERT_EQUAL FAILED " + message.str()); + cout << "ASSERT_EQUAL PASSED " << endl; + } +} + + +void testArm26() { + ForwardTool forward("arm26_Setup_Forward.xml"); + forward.run(); + + Storage results("Results/arm26_states.sto"); + Storage* standard = new Storage(); + string statesFileName("std_arm26_states.sto"); + forward.loadStatesStorage( statesFileName, standard ); + StateVector* state = results.getStateVector(0); + double time = state->getTime(); + Array data; + data.setSize(state->getSize()); + standard->getDataAtTime(time, state->getSize(), data); + 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, + __FILE__, __LINE__, "ASSERT_EQUAL FAILED " + message.str()); + cout << "ASSERT_EQUAL PASSED " << message.str(); + } + + int i = results.getSize()-1; + state = results.getStateVector(i); + time = state->getTime(); + data.setSize(state->getSize()); + standard->getDataAtTime(time, state->getSize(), data); + 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, + __FILE__, __LINE__, "ASSERT_EQUAL FAILED " + message.str()); + cout << "ASSERT_EQUAL PASSED " << message.str(); + } +} + +void testGait2354() +{ + ForwardTool forward("subject01_Setup_Forward.xml"); + forward.run(); + Storage results("Results/subject01_states.sto"); + + //Storage standard("std_subject01_walk1_states.sto"); + 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 rms_tols(2*nstates, 0.001); //activations and fiber-lengths + + for(int i=0; i rms_tols(2*nstates, 0.001); //activations and fiber-lengths + + for(int i=0; iinitSystem(); forward.setModel(*model); model->initSystem(); - forward.run(); // For good measure we'll make sure we still get the identical results diff --git a/OpenSim/Common/Component.h b/OpenSim/Common/Component.h index 3131402ed6..355e352ca4 100644 --- a/OpenSim/Common/Component.h +++ b/OpenSim/Common/Component.h @@ -3506,8 +3506,6 @@ void Socket::finalizeConnection(const Component& root) { const auto& comp = *connectee; const auto& rootOfConnectee = comp.getRoot(); const auto& myRoot = getOwner().getRoot(); -// std::cout << "rootOfConnectee: " << rootOfConnectee.getName() << std::endl; -// std::cout << "myRoot: " << myRoot.getName() << std::endl; if (&myRoot != &rootOfConnectee) { std::stringstream msg; msg << "Socket<" << getConnecteeTypeName() << "> '" << getName() diff --git a/OpenSim/Simulation/Control/ControlSetController.cpp b/OpenSim/Simulation/Control/ControlSetController.cpp index 6f0ff859c2..732b45f558 100644 --- a/OpenSim/Simulation/Control/ControlSetController.cpp +++ b/OpenSim/Simulation/Control/ControlSetController.cpp @@ -277,38 +277,36 @@ void ControlSetController::extendConnectToModel(Model& model) { } // Check that the actuator is connected to the controller. -// bool isConnected = false; -// for (int iactu = 0; iactu < (int)socket.getNumConnectees(); ++iactu) { -// if (socket.getConnectee(iactu).getName() == actName) { -// log_cout("ControlSetController::extendConnectToModel " -// "Actuator '{}' already connected to ControlSetController '{}'.", -// actName, getName()); -// isConnected = true; -// break; -// } -// } - - // If not already connected, try to connect to an actuator in the model. -// if (!isConnected) { - updSocket("actuators").disconnect(); - - for (const auto& actu : model.getComponentList()) { - if (actu.getName() == actName) { + bool isConnected = false; + for (int iactu = 0; iactu < (int)socket.getNumConnectees(); ++iactu) { + if (socket.getConnectee(iactu).getName() == actName) { log_cout("ControlSetController::extendConnectToModel " - "Connecting ControlSetController '{}' to Actuator" - "'{}'.", getName(), actu.getName()); - addActuator(actu); -// isConnected = true; + "Actuator '{}' already connected to ControlSetController '{}'.", + actName, getName()); + isConnected = true; break; } } -// } + + // If not already connected, try to connect to an actuator in the model. + if (!isConnected) { + for (const auto& actu : model.getComponentList()) { + if (actu.getName() == actName) { + log_cout("ControlSetController::extendConnectToModel " + "Connecting ControlSetController '{}' to Actuator" + "'{}'.", getName(), actu.getName()); + addActuator(actu); + isConnected = true; + break; + } + } + } updSocket("actuators").finalizeConnection(model); -// OPENSIM_THROW_IF_FRMOBJ(!isConnected, Exception, -// "Control with name '{}' provided in the ControlSet '{}', but no " -// "matching Actuator was found in the model.", -// actName, _controlSet->getName()); + OPENSIM_THROW_IF_FRMOBJ(!isConnected, Exception, + "Control with name '{}' provided in the ControlSet '{}', but no " + "matching Actuator was found in the model.", + actName, _controlSet->getName()); } } diff --git a/OpenSim/Tools/ForwardTool.cpp b/OpenSim/Tools/ForwardTool.cpp index 70be288471..f410c15be4 100644 --- a/OpenSim/Tools/ForwardTool.cpp +++ b/OpenSim/Tools/ForwardTool.cpp @@ -246,12 +246,6 @@ bool ForwardTool::run() std::cout << "DEBUG: ForwardTool::run() _externalLoadsFileName = " << _externalLoadsFileName << std::endl; /*bool externalLoads = */createExternalLoads(_externalLoadsFileName, *_model); -// if (_externalLoadsFileName.empty()) { -// log_warn("External loads file '{}' is ignored.", _externalLoadsFileName); -// } else { -// auto* externalLoads = new ExternalLoads(_externalLoadsFileName, true); -// _model->addModelComponent(externalLoads); -// } // Re create the system with forces above and Realize the topology SimTK::State& s = _model->initSystem(); From c82227f7d1e6bb694a333b53bead6ea66dff06c8 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 22 Jan 2024 11:16:10 -0800 Subject: [PATCH 31/48] Undo debugging code in AbstractTool for copied controllers --- OpenSim/Simulation/Model/AbstractTool.cpp | 14 -------------- 1 file changed, 14 deletions(-) diff --git a/OpenSim/Simulation/Model/AbstractTool.cpp b/OpenSim/Simulation/Model/AbstractTool.cpp index 7e333929e0..dcac598069 100644 --- a/OpenSim/Simulation/Model/AbstractTool.cpp +++ b/OpenSim/Simulation/Model/AbstractTool.cpp @@ -462,22 +462,8 @@ addControllerSetToModel() int size = _controllerSet.getSize(); _controllerCopies.setMemoryOwner(false); - std::cout << "DEBUG controller set size: " << size << std::endl; for(int i=0;i("actuators").disconnect(); Controller *controller = _controllerSet.get(i).clone(); - const auto& socket = _controllerSet.get(i).getSocket("actuators"); - std::cout << "DEBUG num connectees: " << socket.getNumConnectees() << std::endl; - const auto& socketCopy = controller->updSocket("actuators"); - for (int j = 0; i < socket.getNumConnectees(); ++i) { - const auto& actuator = socket.getConnectee(j); - const auto& actuatorCopy = socketCopy.getConnectee(j); - - std::cout << "DEBUG connectee path: " << socket.getConnecteePath(j) << std::endl; - std::cout << "DEBUG connectee path copy: " << socketCopy.getConnecteePath(j) << std::endl; - - } - _model->addController(controller); _controllerCopies.adoptAndAppend(controller); } From 3e0ded6561763ffa69ab1116df5ceb562f6b9f6c Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 22 Jan 2024 12:33:36 -0800 Subject: [PATCH 32/48] Fix testForward --- Applications/Forward/test/testForward.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/Applications/Forward/test/testForward.cpp b/Applications/Forward/test/testForward.cpp index d62586995b..d1d4b71e8f 100644 --- a/Applications/Forward/test/testForward.cpp +++ b/Applications/Forward/test/testForward.cpp @@ -241,10 +241,12 @@ void testGait2354WithControllerGUI() { forward.setResultsDir(resultsDir); forward.updateModelForces(*model, ""); - model->initSystem(); - forward.setModel(*model); + // This initSystem() call needs to happen after updateModelForces() and + // before setModel(). Calling initSystem() after setModel() will cause + // Controller connectee paths to be incorrect. model->initSystem(); + forward.setModel(*model); forward.run(); // For good measure we'll make sure we still get the identical results From 3a895dbac5796c6cf7589009cdd4ef601dc535df Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 22 Jan 2024 12:44:15 -0800 Subject: [PATCH 33/48] Convert to int to avoid warnings treated as errors --- OpenSim/ExampleComponents/ToyReflexController.cpp | 4 ++-- OpenSim/Tests/Wrapping/testWrapping.cpp | 2 +- OpenSim/Tools/CMC.cpp | 2 +- OpenSim/Tools/ForwardTool.cpp | 1 - 4 files changed, 4 insertions(+), 5 deletions(-) diff --git a/OpenSim/ExampleComponents/ToyReflexController.cpp b/OpenSim/ExampleComponents/ToyReflexController.cpp index 82a4659938..275645e5e9 100644 --- a/OpenSim/ExampleComponents/ToyReflexController.cpp +++ b/OpenSim/ExampleComponents/ToyReflexController.cpp @@ -67,7 +67,7 @@ void ToyReflexController::extendConnectToModel(Model &model) Super::extendConnectToModel(model); const auto& socket = getSocket("actuators"); - for (int i = 0; i < socket.getNumConnectees(); ++i) { + for (int i = 0; i < (int)socket.getNumConnectees(); ++i) { const auto& actu = socket.getConnectee(i); const auto* musc = dynamic_cast(&actu); OPENSIM_THROW_IF_FRMOBJ(!musc, Exception, @@ -100,7 +100,7 @@ void ToyReflexController::computeControls(const State& s, //reflex control double control = 0; - for (int i = 0; i < socket.getNumConnectees(); ++i) { + for (int i = 0; i < (int)socket.getNumConnectees(); ++i) { const auto& actu = socket.getConnectee(i); const Muscle *musc = dynamic_cast(&actu); speed = musc->getLengtheningSpeed(s); diff --git a/OpenSim/Tests/Wrapping/testWrapping.cpp b/OpenSim/Tests/Wrapping/testWrapping.cpp index a78b809465..2dd0237d1f 100644 --- a/OpenSim/Tests/Wrapping/testWrapping.cpp +++ b/OpenSim/Tests/Wrapping/testWrapping.cpp @@ -85,7 +85,7 @@ namespace { const auto& actuatorsSet = model.getActuators(); actuController.setActuators(actuatorsSet); const auto& socket = actuController.getSocket("actuators"); - for (int i = 0; i < socket.getNumConnectees(); i++) { + for (int i = 0; i < (int)socket.getNumConnectees(); i++) { actuController.prescribeControlForActuator( actuatorsSet.get(i).getAbsolutePathString(), new Constant(activation)); diff --git a/OpenSim/Tools/CMC.cpp b/OpenSim/Tools/CMC.cpp index dda4c3ffd5..fe02b950d9 100644 --- a/OpenSim/Tools/CMC.cpp +++ b/OpenSim/Tools/CMC.cpp @@ -1100,7 +1100,7 @@ void CMC::computeControls(const SimTK::State& s, SimTK::Vector& controls) const SimTK::Vector actControls(1, 0.0); const auto& socket = getSocket("actuators"); - for(int i = 0; i < socket.getNumConnectees(); i++){ + for(int i = 0; i < (int)socket.getNumConnectees(); i++){ actControls[0] = _controlSet[_controlSetIndices[i]].getControlValue(s.getTime()); socket.getConnectee(i).addInControls(actControls, controls); } diff --git a/OpenSim/Tools/ForwardTool.cpp b/OpenSim/Tools/ForwardTool.cpp index f410c15be4..79216b38c1 100644 --- a/OpenSim/Tools/ForwardTool.cpp +++ b/OpenSim/Tools/ForwardTool.cpp @@ -244,7 +244,6 @@ bool ForwardTool::run() // so that the parsing code behaves properly if called from a different directory. auto cwd = IO::CwdChanger::changeToParentOf(getDocumentFileName()); - std::cout << "DEBUG: ForwardTool::run() _externalLoadsFileName = " << _externalLoadsFileName << std::endl; /*bool externalLoads = */createExternalLoads(_externalLoadsFileName, *_model); // Re create the system with forces above and Realize the topology From 196d342bd429b63a77acc3029938436dcd6693f4 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 22 Jan 2024 13:57:41 -0800 Subject: [PATCH 34/48] Remove unnecessary addActuator() calls --- OpenSim/Simulation/Test/testInitState.cpp | 3 +-- OpenSim/Tools/Test/testControllers.cpp | 17 +++++++---------- 2 files changed, 8 insertions(+), 12 deletions(-) diff --git a/OpenSim/Simulation/Test/testInitState.cpp b/OpenSim/Simulation/Test/testInitState.cpp index 9cf119d7dc..3d838f24c8 100644 --- a/OpenSim/Simulation/Test/testInitState.cpp +++ b/OpenSim/Simulation/Test/testInitState.cpp @@ -71,8 +71,7 @@ void testStates(const string& modelFile) Model model(modelFile); ControlSetController* controller = new ControlSetController(); controller->setControlSetFileName("arm26_StaticOptimization_controls.xml"); - controller->setActuators(model.getComponentList()); - + model.addController( controller ); // original default state State& state = model.initSystem(); diff --git a/OpenSim/Tools/Test/testControllers.cpp b/OpenSim/Tools/Test/testControllers.cpp index 4b67d723ec..9d2bbbdfff 100644 --- a/OpenSim/Tools/Test/testControllers.cpp +++ b/OpenSim/Tools/Test/testControllers.cpp @@ -7,7 +7,7 @@ * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * * through the Warrior Web program. * * * - * Copyright (c) 2005-2017 Stanford University and the Authors * + * Copyright (c) 2005-2023 Stanford University and the Authors * * Author(s): Ajay Seth * * * * Licensed under the Apache License, Version 2.0 (the "License"); you may * @@ -26,10 +26,11 @@ // that controllers behave as described. // // Tests Include: -// 1. Test a ControlSetController on a block with an ideal actuator -// 2. Test a PrescribedController on a block with an ideal actuator -// 3. Test a CorrectionController tracking a block with an ideal actuator -// 4. Test a PrescribedController on the arm26 model with reserves. +// 1. Test the Controller interface +// 2. Test a ControlSetController on a block with an ideal actuator +// 3. Test a PrescribedController on a block with an ideal actuator +// 4. Test a CorrectionController tracking a block with an ideal actuator +// 5. Test a PrescribedController on the arm26 model with reserves. // Add tests here as new controller types are added to OpenSim // //============================================================================= @@ -131,8 +132,7 @@ TEST_CASE("testControlSetControllerOnBlock") { actuatorControls.setControlValues(finalTime, controlForce); // Create a control set controller that simply applies controls from a ControlSet auto* actuatorController = new ControlSetController(); - actuatorController->addActuator( - osimModel.getComponent("/forceset/actuator")); + // Make a copy and set it on the ControlSetController as it takes ownership of the // ControlSet passed in actuatorController->setControlSet( @@ -345,7 +345,6 @@ TEST_CASE("testPrescribedControllerFromFile") { ControlSetController csc; ControlSet* cs = new ControlSet(controlsFile); csc.setControlSet(cs); - csc.setActuators(osimModel.updActuators()); // add the controller to the model osimModel.addController(&csc); @@ -385,11 +384,9 @@ TEST_CASE("testPrescribedControllerFromFile") { //************* Rerun with a PrescribedController ***********************/ PrescribedController prescribed(outfileName, 1); - prescribed.setActuators(osimModel.updActuators()); // add the controller to the model osimModel.addController(&prescribed); - osimModel.finalizeConnections(); osimModel.print("TestPrescribedController.osim"); // Initialize the system and get the state representing the state system From a4f47b66ee30505b0858d224f9d57c7630d60091 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Wed, 24 Jan 2024 10:48:31 -0800 Subject: [PATCH 35/48] Remove arrays of ReferencePtrs --- OpenSim/Simulation/Control/Controller.cpp | 20 -------------------- OpenSim/Simulation/Control/Controller.h | 13 ------------- OpenSim/Tools/Test/testControllers.cpp | 13 ++----------- 3 files changed, 2 insertions(+), 44 deletions(-) diff --git a/OpenSim/Simulation/Control/Controller.cpp b/OpenSim/Simulation/Control/Controller.cpp index 9bff4f6505..2e517f0f01 100644 --- a/OpenSim/Simulation/Control/Controller.cpp +++ b/OpenSim/Simulation/Control/Controller.cpp @@ -152,13 +152,6 @@ void Controller::setActuators(const Set& actuators) { } } -void Controller::setActuators( - const SimTK::Array_>& actuators) { - updSocket("actuators").disconnect(); - for (const auto& actu : actuators) { - addActuator(actu.getRef()); - } -} void Controller::setActuators(const ComponentList& actuators) { updSocket("actuators").disconnect(); @@ -170,16 +163,3 @@ void Controller::setActuators(const ComponentList& actuators) { void Controller::addActuator(const Actuator& actuator) { appendSocketConnectee_actuators(actuator); } - -SimTK::Array_> -Controller::getActuators() const { - const auto& socket = getSocket("actuators"); - int nc = static_cast(socket.getNumConnectees()); - SimTK::Array_> actuators(nc); - for (int i = 0; i < (int)socket.getNumConnectees(); ++i) { - const Actuator& actu = socket.getConnectee(i); - actuators[i] = &actu; - } - - return actuators; -} diff --git a/OpenSim/Simulation/Control/Controller.h b/OpenSim/Simulation/Control/Controller.h index 59871b980c..012cef9eab 100644 --- a/OpenSim/Simulation/Control/Controller.h +++ b/OpenSim/Simulation/Control/Controller.h @@ -62,14 +62,6 @@ class Actuator; * * // Add a ComponentList of Actuators to the controller. * controller.setActuators(model.getComponentList()); - * - * // Add an array of specific Actuator reference pointers to the controller. - * const auto& actuator1 = model.getComponent("/path/to/actuator1"); - * const auto& actuator2 = model.getComponent("/path/to/actuator2"); - * SimTK::Array_> actuators(2); - * actuators[0] = &actuator1; - * actuators[1] = &actuator2; - * controller.setActuators(actuators); * @endcode * * @note Prior to OpenSim 4.6, controlled actuators were managed via the list @@ -126,16 +118,11 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Controller, ModelComponent); /** Replace the current set of actuators with the provided set. */ void setActuators(const Set& actuators); - void setActuators( - const SimTK::Array_>& actuators); void setActuators(const ComponentList& actuators); /** Add to the current set of actuators. */ void addActuator(const Actuator& actuator); - /** Get an array of reference pointers to the current set of actuators. */ - SimTK::Array_> getActuators() const; - /** Compute the control for actuator * This method defines the behavior for any concrete controller * and therefore must be implemented by concrete subclasses. diff --git a/OpenSim/Tools/Test/testControllers.cpp b/OpenSim/Tools/Test/testControllers.cpp index 9d2bbbdfff..d570221ccd 100644 --- a/OpenSim/Tools/Test/testControllers.cpp +++ b/OpenSim/Tools/Test/testControllers.cpp @@ -59,22 +59,13 @@ TEST_CASE("Test Controller interface") { controller->addActuator(model.getComponent("/tau0")); controller->addActuator(model.getComponent("/tau1")); model.addController(controller); - REQUIRE_NOTHROW(model.finalizeConnections()); + model.finalizeConnections(); } SECTION("Adding multiple actuators from a ComponentList") { controller->setActuators(model.getComponentList()); model.addController(controller); - REQUIRE_NOTHROW(model.finalizeConnections()); - } - - SECTION("Adding multiple actuators from ReferencePtrs") { - SimTK::Array_> actuators(2); - actuators[0] = &model.getComponent("/tau0"); - actuators[1] = &model.getComponent("/tau1"); - controller->setActuators(actuators); - model.addController(controller); - REQUIRE_NOTHROW(model.finalizeConnections()); + model.finalizeConnections(); } } From f1687f2507f854d587008fe110718fe0c88110bf Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Wed, 24 Jan 2024 14:26:31 -0800 Subject: [PATCH 36/48] Switch to list Sockets for Controller actuators --- OpenSim/Moco/MocoProblemRep.cpp | 7 ++++--- OpenSim/Simulation/Control/DiscreteController.cpp | 8 ++++++-- OpenSim/Simulation/Control/InputController.cpp | 6 +++--- OpenSim/Simulation/SimulationUtilities.cpp | 12 ++++++------ 4 files changed, 19 insertions(+), 14 deletions(-) diff --git a/OpenSim/Moco/MocoProblemRep.cpp b/OpenSim/Moco/MocoProblemRep.cpp index 410c99bf31..5469806eb5 100644 --- a/OpenSim/Moco/MocoProblemRep.cpp +++ b/OpenSim/Moco/MocoProblemRep.cpp @@ -511,9 +511,10 @@ void MocoProblemRep::initialize() { // Loop through the actuators in the DiscreteController since these are the // only actuators that will have associated optimal control variables. // Create control infos for actuators that do not have a any control info. - const auto& dcActuators = m_discrete_controller_base->getActuatorSet(); - for (int i = 0; i < dcActuators.getSize(); ++i) { - const auto& actu = dcActuators.get(i); + const auto& dcSocket = + m_discrete_controller_base->getSocket("actuators"); + for (int i = 0; i < dcSocket.getNumConnectees(); ++i) { + const auto& actu = dcSocket.getConnectee(i); const std::string actuName = actu.getAbsolutePathString(); if (actu.numControls() == 1) { // No control info exists; add one. diff --git a/OpenSim/Simulation/Control/DiscreteController.cpp b/OpenSim/Simulation/Control/DiscreteController.cpp index b25763ac5b..b5b35cad41 100644 --- a/OpenSim/Simulation/Control/DiscreteController.cpp +++ b/OpenSim/Simulation/Control/DiscreteController.cpp @@ -67,8 +67,12 @@ void DiscreteController::extendRealizeTopology(SimTK::State& state) const { for (const auto& actu : getModel().getComponentList()) { const int nc = actu.numControls(); for (int ic = 0; ic < nc; ++ic) { - if (getActuatorSet().contains(actu.getName())) { - m_controlIndices.push_back(count); + const auto& socket = getSocket("actuators"); + for (int i = 0; i < socket.getNumConnectees(); ++i) { + if (socket.getConnecteePath(i) == actu.getAbsolutePath()) { + m_controlIndices.push_back(count);; + break; + } } count++; } diff --git a/OpenSim/Simulation/Control/InputController.cpp b/OpenSim/Simulation/Control/InputController.cpp index 5e00a48597..4f85d0fedc 100644 --- a/OpenSim/Simulation/Control/InputController.cpp +++ b/OpenSim/Simulation/Control/InputController.cpp @@ -96,9 +96,9 @@ void InputController::extendConnectToModel(Model& model) { // names and their indexes in the model control cache. m_controlNames.clear(); m_controlIndexes.clear(); - const auto& actuSet = getActuatorSet(); - for (int i = 0; i < actuSet.getSize(); ++i) { - const auto& actu = actuSet.get(i); + const auto& socket = getSocket("actuators"); + for (int i = 0; i < socket.getNumConnectees(); ++i) { + const auto& actu = socket.getConnectee(i); if (actu.numControls() > 1) { // Non-scalar actuator. for (int j = 0; j < actu.numControls(); ++j) { diff --git a/OpenSim/Simulation/SimulationUtilities.cpp b/OpenSim/Simulation/SimulationUtilities.cpp index 2081e4d731..488a69dbff 100644 --- a/OpenSim/Simulation/SimulationUtilities.cpp +++ b/OpenSim/Simulation/SimulationUtilities.cpp @@ -314,9 +314,9 @@ std::vector OpenSim::createControlledActuatorPathsFromModel( ignoreDiscreteController) { continue; } - const auto& actuators = controller.getActuatorSet(); - for (int i = 0; i < actuators.getSize(); ++i) { - const auto& actu = actuators.get(i); + const auto& socket = controller.getSocket("actuators"); + for (int i = 0; i < socket.getNumConnectees(); ++i) { + const auto& actu = socket.getConnectee(i); const auto& actuPath = actu.getAbsolutePathString(); if (std::find(actuPaths.begin(), actuPaths.end(), actuPath) == actuPaths.end()) { @@ -330,9 +330,9 @@ std::vector OpenSim::createControlledActuatorPathsFromModel( if (ignoreDiscreteController) { for (const auto& controller : model.getComponentList()) { - const auto& actuators = controller.getActuatorSet(); - for (int i = 0; i < actuators.getSize(); ++i) { - const auto& actu = actuators.get(i); + const auto& socket = controller.getSocket("actuators"); + for (int i = 0; i < socket.getNumConnectees(); ++i) { + const auto& actu = socket.getConnectee(i); const auto& actuPath = actu.getAbsolutePathString(); auto it = std::find(actuPaths.begin(), actuPaths.end(), actuPath); From 7198b6ecb4efd8d914dbabb1cace9e625f7ae8c8 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Thu, 25 Jan 2024 10:15:16 -0800 Subject: [PATCH 37/48] Add ControlAllocator // revert stuff in SimulationUtilities --- OpenSim/Moco/CMakeLists.txt | 2 + OpenSim/Moco/Components/ControlAllocator.cpp | 90 +++++++++++++ OpenSim/Moco/Components/ControlAllocator.h | 94 +++++++++++++ .../Moco/MocoCasADiSolver/MocoCasOCProblem.h | 5 + OpenSim/Moco/MocoProblemRep.cpp | 54 ++++---- OpenSim/Moco/MocoProblemRep.h | 26 ++-- OpenSim/Moco/RegisterTypes_osimMoco.cpp | 2 + OpenSim/Moco/osimMoco.h | 1 + OpenSim/Sandbox/Moco/sandboxSandbox.cpp | 123 +----------------- OpenSim/Simulation/Control/Controller.h | 10 -- .../Simulation/Control/InputController.cpp | 36 ++--- OpenSim/Simulation/Control/InputController.h | 2 +- OpenSim/Simulation/SimulationUtilities.cpp | 72 +--------- OpenSim/Simulation/SimulationUtilities.h | 26 +--- 14 files changed, 264 insertions(+), 279 deletions(-) create mode 100644 OpenSim/Moco/Components/ControlAllocator.cpp create mode 100644 OpenSim/Moco/Components/ControlAllocator.h diff --git a/OpenSim/Moco/CMakeLists.txt b/OpenSim/Moco/CMakeLists.txt index 580daf9064..ad34b56d9c 100644 --- a/OpenSim/Moco/CMakeLists.txt +++ b/OpenSim/Moco/CMakeLists.txt @@ -88,6 +88,8 @@ set(MOCO_SOURCES Components/DiscreteForces.cpp Components/AccelerationMotion.h Components/AccelerationMotion.cpp + Components/ControlAllocator.h + Components/ControlAllocator.cpp MocoCasADiSolver/MocoCasADiSolver.h MocoCasADiSolver/MocoCasADiSolver.cpp MocoInverse.cpp diff --git a/OpenSim/Moco/Components/ControlAllocator.cpp b/OpenSim/Moco/Components/ControlAllocator.cpp new file mode 100644 index 0000000000..fa140e542d --- /dev/null +++ b/OpenSim/Moco/Components/ControlAllocator.cpp @@ -0,0 +1,90 @@ +/* -------------------------------------------------------------------------- * + * OpenSim: ControlAllocator.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 "ControlAllocator.h" + +using namespace OpenSim; + +//============================================================================= +// CONSTRUCTOR(S) AND DESTRUCTOR +//============================================================================= +ControlAllocator::ControlAllocator() = default; + +ControlAllocator::~ControlAllocator() noexcept = default; + +ControlAllocator::ControlAllocator(const ControlAllocator&) = default; + +ControlAllocator::ControlAllocator(ControlAllocator&&) = default; + +ControlAllocator& ControlAllocator::operator=(const ControlAllocator&) = default; + +ControlAllocator& ControlAllocator::operator=(ControlAllocator&&) = default; + +//============================================================================= +// GET AND SET +//============================================================================= +void ControlAllocator::addControl(const std::string& controlName) { + const int index = (int)m_controlIndexMap.size(); + m_controlIndexMap[controlName] = index; +} + +void ControlAllocator::setControls(SimTK::State& s, + const SimTK::Vector& controls) const { + updControls(s) = controls; +} + +SimTK::Vector& ControlAllocator::updControls(SimTK::State& s) const { + const SimTK::Subsystem& subSys = getSystem().getDefaultSubsystem(); + auto& dv = subSys.updDiscreteVariable(s, m_discreteVarIndex); + auto& discreteControls = + SimTK::Value::updDowncast(dv).upd(); + return discreteControls; +} + +const SimTK::Vector& ControlAllocator::getControls( + const SimTK::State& s) const { + const SimTK::Subsystem& subSys = getSystem().getDefaultSubsystem(); + auto& dv = subSys.getDiscreteVariable(s, m_discreteVarIndex); + auto& discreteControls = SimTK::Value::downcast(dv).get(); + return discreteControls; +} + +double ControlAllocator::getControlForOutputChannel(const SimTK::State& s, + const std::string& channel) const { + return getControls(s)[m_controlIndexMap.at(channel)]; +} + +//============================================================================= +// MODEL COMPONENT INTERFACE +//============================================================================= +void ControlAllocator::extendRealizeTopology(SimTK::State& state) const { + Super::extendRealizeTopology(state); + const SimTK::Subsystem& subSys = getSystem().getDefaultSubsystem(); + const SimTK::Vector initControls( + static_cast(m_controlIndexMap.size()), 0.0); + m_discreteVarIndex = + subSys.allocateDiscreteVariable(state, SimTK::Stage::Dynamics, + new SimTK::Value(initControls)); +} + +void ControlAllocator::extendFinalizeFromProperties() { + Super::extendFinalizeFromProperties(); + for (const auto& kv : m_controlIndexMap) { + updOutput("controls").addChannel(kv.first); + } +} diff --git a/OpenSim/Moco/Components/ControlAllocator.h b/OpenSim/Moco/Components/ControlAllocator.h new file mode 100644 index 0000000000..10aadf373f --- /dev/null +++ b/OpenSim/Moco/Components/ControlAllocator.h @@ -0,0 +1,94 @@ +#ifndef OPENSIM_CONTROLALLOCATOR_H +#define OPENSIM_CONTROLALLOCATOR_H +/* -------------------------------------------------------------------------- * + * OpenSim: ControlAllocator.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 +#include + +namespace OpenSim { + +class OSIMMOCO_API ControlAllocator : public ModelComponent { + OpenSim_DECLARE_CONCRETE_OBJECT(ControlAllocator, ModelComponent); + +public: +//============================================================================= +// OUTPUTS +//============================================================================= + OpenSim_DECLARE_LIST_OUTPUT(controls, double, getControlForOutputChannel, + SimTK::Stage::Dynamics); + +//============================================================================= +// METHODS +//============================================================================= + + // CONSTRUCTION AND DESTRUCTION + ControlAllocator(); + ~ControlAllocator() noexcept override; + + ControlAllocator(const ControlAllocator&); + ControlAllocator(ControlAllocator&&); + + ControlAllocator& operator=(const ControlAllocator&); + ControlAllocator& operator=(ControlAllocator&&); + + // GET AND SET + /** + * Add a control to the control allocator. + */ + void addControl(const std::string& controlName); + + /** + * Set the allocator controls stored in the discrete variable. + * + * The `controls` vector must be the same size as the number of controls + * added to the allocator by `addControl()`. + */ + void setControls(SimTK::State& s, const SimTK::Vector& controls) const; + + /** + * Get a writable reference to the allocator controls stored in the discrete + * variable. + */ + SimTK::Vector& updControls(SimTK::State& s) const; + + /** + * Get a const reference to the allocator 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; + +protected: + // MODEL COMPONENT INTERFACE + void extendRealizeTopology(SimTK::State& state) const override; + void extendFinalizeFromProperties() override; + +private: + std::unordered_map m_controlIndexMap; + mutable SimTK::DiscreteVariableIndex m_discreteVarIndex; +}; + +} // namespace OpenSim + +#endif // OPENSIM_CONTROLALLOCATOR_H diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h index b0a080f127..278492b4f9 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h @@ -557,6 +557,11 @@ class MocoCasOCProblem : public CasOC::Problem { if (stageDep >= SimTK::Stage::Model) { convertStatesToSimTKState( stageDep, time, states, model, simtkState, true); + + // TODO replace with ControlAllocator + // TODO set controls to ControlAllocator in problem order? the + // controls will need to match the order of the controls + // add to ControlAllocator SimTK::Vector& simtkControls = discreteController.updDiscreteControls(simtkState); for (int ic = 0; ic < getNumControls(); ++ic) { diff --git a/OpenSim/Moco/MocoProblemRep.cpp b/OpenSim/Moco/MocoProblemRep.cpp index 5469806eb5..47a00b5864 100644 --- a/OpenSim/Moco/MocoProblemRep.cpp +++ b/OpenSim/Moco/MocoProblemRep.cpp @@ -20,6 +20,7 @@ #include "Components/AccelerationMotion.h" #include "Components/DiscreteForces.h" +#include "Components/ControlAllocator.h" #include "MocoProblem.h" #include "MocoProblemInfo.h" #include "MocoScaleFactor.h" @@ -28,8 +29,8 @@ #include #include -#include #include +#include using namespace OpenSim; @@ -102,17 +103,15 @@ void MocoProblemRep::initialize() { checkOrderSystemControls(m_model_base); // Steps: - // 1) Check that existing controllers are valid (PrescribedController and ActuatorController, for now) + // 1) Check that existing controllers are valid (PrescribedController, for now) // 2) Skip over actuators that have controllers (when the user does not want to add controls for these actuators) - // 3) Add a ControlAllocator (replace DiscreteController) + // 3) Add a ControlAllocator // TODO need a MocoProblem option to allow adding OCP controls for // that already have a controller. // Check that the any controllers added by the user are valid. - bool addControlsForControlledActuators = false; - std::vector actuatorsToSkip; - std::vector controlNamesToAddToControlDistributor; // TODO better name + std::vector controlNamesToAddToControlAllocator; // TODO better name for (const auto& controller : m_model_base.getComponentList()) { if (!dynamic_cast(&controller)) { OPENSIM_THROW(Exception, "Moco only supports PrescribedController " @@ -120,12 +119,6 @@ void MocoProblemRep::initialize() { "'{}'.", controller.getAbsolutePathString(), controller.getConcreteClassName()); - - // if (!addControlsForControlledActuators) { - // for (const auto& actu : controller.getActuatorSet()) { - // actuatorsToSkip.push_back(actu.getAbsolutePathString()); - // } - // } } } @@ -135,21 +128,24 @@ void MocoProblemRep::initialize() { std::vector controlledActuatorPaths = createControlledActuatorPathsFromModel(m_model_base, false); - // Add the non-controlled, enabled actuators to the DiscreteController. - auto discreteControllerBaseUPtr = make_unique(); - discreteControllerBaseUPtr->setName("discrete_controller"); + // Add the non-controlled, enabled actuators to an ActuatorInputController. + auto actuatorController = make_unique(); + actuatorController->setName("actuator_controller"); for (const auto& actu : m_model_base.getComponentList()) { bool isControlled = std::find(controlledActuatorPaths.begin(), controlledActuatorPaths.end(), actu.getAbsolutePathString()) != controlledActuatorPaths.end(); if (!isControlled && actu.get_appliesForce()) { - discreteControllerBaseUPtr->addActuator(actu); + actuatorController->addActuator(actu); } } + m_model_base.addController(actuatorController.release()); - m_discrete_controller_base.reset(discreteControllerBaseUPtr.get()); - m_model_base.addController(discreteControllerBaseUPtr.release()); + // Add a ControlAllocator to the model. + auto controlAllocatorUPtr = make_unique(); + m_control_allocator_base.reset(controlAllocatorUPtr.get()); + m_model_base.addComponent(controlAllocatorUPtr.release()); // Scale factors // ------------- @@ -240,8 +236,8 @@ void MocoProblemRep::initialize() { m_model_disabled_constraints.addComponent(constraintForcesUPtr.release()); m_model_disabled_constraints.finalizeFromProperties(); - m_discrete_controller_disabled_constraints.reset( - &*m_model_disabled_constraints.getComponentList().begin()); + m_control_allocator_disabled_constraints.reset( + &*m_model_disabled_constraints.getComponentList().begin()); if (!m_prescribedKinematics) { @@ -508,13 +504,17 @@ void MocoProblemRep::initialize() { m_control_infos[name] = ph0.get_control_infos(i); } - // Loop through the actuators in the DiscreteController since these are the - // only actuators that will have associated optimal control variables. - // Create control infos for actuators that do not have a any control info. - const auto& dcSocket = - m_discrete_controller_base->getSocket("actuators"); - for (int i = 0; i < dcSocket.getNumConnectees(); ++i) { - const auto& actu = dcSocket.getConnectee(i); + // Loop through the actuators in the ActuatorInputController since these are + // the only actuators that will have associated optimal control variables. + // Create control infos for actuators that do not have a control info. + // TODO this will change if we allow OCP controls on top of controls from + // controllers. + const auto& actuController = + m_model_base.getComponent( + "actuator_controller"); + const auto& socket = actuController.getSocket("actuators"); + for (int i = 0; i < socket.getNumConnectees(); ++i) { + const auto& actu = socket.getConnectee(i); const std::string actuName = actu.getAbsolutePathString(); if (actu.numControls() == 1) { // No control info exists; add one. diff --git a/OpenSim/Moco/MocoProblemRep.h b/OpenSim/Moco/MocoProblemRep.h index c00ef5d1db..fd1125ff93 100644 --- a/OpenSim/Moco/MocoProblemRep.h +++ b/OpenSim/Moco/MocoProblemRep.h @@ -31,7 +31,8 @@ namespace OpenSim { class MocoProblem; -class DiscreteController; +//class DiscreteController; +class ControlAllocator; class DiscreteForces; class PositionMotion; class AccelerationMotion; @@ -89,8 +90,11 @@ class OSIMMOCO_API MocoProblemRep { SimTK::State& updStateBase() const { return m_state_base; } /// This is a component inside ModelBase that you can use to /// set the value of control signals. - const DiscreteController& getDiscreteControllerBase() const { - return m_discrete_controller_base.getRef(); +// const DiscreteController& getDiscreteControllerBase() const { +// return m_discrete_controller_base.getRef(); +// } + const ControlAllocator& getControlAllocatorBase() const { + return m_control_allocator_base.getRef(); } /// Get a reference to a copy of the model being used by this /// MocoProblemRep, but with all constraints disabled and an additional @@ -117,8 +121,11 @@ class OSIMMOCO_API MocoProblemRep { } /// This is a component inside ModelDisabledConstraints that you can use to /// set the value of control signals. - const DiscreteController& getDiscreteControllerDisabledConstraints() const { - return m_discrete_controller_disabled_constraints.getRef(); +// const DiscreteController& getDiscreteControllerDisabledConstraints() const { +// return m_discrete_controller_disabled_constraints.getRef(); +// } + const ControlAllocator& getControlAllocatorDisabledConstraints() const { + return m_control_allocator_disabled_constraints.getRef(); } /// This is a component inside ModelDisabledConstraints that you can use /// to set the value of discrete forces, intended to hold the constraint @@ -379,13 +386,16 @@ class OSIMMOCO_API MocoProblemRep { Model m_model_base; mutable SimTK::State m_state_base; - SimTK::ReferencePtr m_discrete_controller_base; + //SimTK::ReferencePtr m_discrete_controller_base; + SimTK::ReferencePtr m_control_allocator_base; SimTK::ReferencePtr m_position_motion_base; Model m_model_disabled_constraints; mutable std::array m_state_disabled_constraints; - SimTK::ReferencePtr - m_discrete_controller_disabled_constraints; +// SimTK::ReferencePtr +// m_discrete_controller_disabled_constraints; + SimTK::ReferencePtr + m_control_allocator_disabled_constraints; SimTK::ReferencePtr m_position_motion_disabled_constraints; SimTK::ReferencePtr m_constraint_forces; diff --git a/OpenSim/Moco/RegisterTypes_osimMoco.cpp b/OpenSim/Moco/RegisterTypes_osimMoco.cpp index 2056ad5cdb..54153a48cc 100644 --- a/OpenSim/Moco/RegisterTypes_osimMoco.cpp +++ b/OpenSim/Moco/RegisterTypes_osimMoco.cpp @@ -20,6 +20,7 @@ #include "Components/AccelerationMotion.h" #include "Components/DiscreteForces.h" #include "Components/StationPlaneContactForce.h" +#include "Components/ControlAllocator.h" #include "MocoBounds.h" #include "MocoScaleFactor.h" #include "MocoCasADiSolver/MocoCasADiSolver.h" @@ -124,6 +125,7 @@ OSIMMOCO_API void RegisterTypes_osimMoco() { Object::registerType(DiscreteForces()); Object::registerType(AccelerationMotion()); + Object::registerType(ControlAllocator()); } catch (const std::exception& e) { std::cerr << "ERROR during osimMoco Object registration:\n" diff --git a/OpenSim/Moco/osimMoco.h b/OpenSim/Moco/osimMoco.h index 493c5dcd9e..4aca244a40 100644 --- a/OpenSim/Moco/osimMoco.h +++ b/OpenSim/Moco/osimMoco.h @@ -21,6 +21,7 @@ #include "About.h" #include "Components/DiscreteForces.h" #include "Components/StationPlaneContactForce.h" +#include "Components/ControlAllocator.h" #include "MocoBounds.h" #include "MocoCasADiSolver/MocoCasADiSolver.h" #include "MocoConstraint.h" diff --git a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp index 858378c25c..0ddd4eecb4 100644 --- a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp +++ b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp @@ -21,130 +21,13 @@ #include "OpenSim/Actuators/CoordinateActuator.h" #include "OpenSim/Actuators/ModelFactory.h" -#include "OpenSim/Simulation/Control/DiscreteController.h" #include +#include #include using namespace OpenSim; -class ControlAllocator : public Component { - OpenSim_DECLARE_CONCRETE_OBJECT(ControlAllocator, Component); -public: - // LIST OUTPUT - OpenSim_DECLARE_LIST_OUTPUT( - controls, double, getControlForOutputChannel, SimTK::Stage::Dynamics); - - // CONSTRUCTION - ControlAllocator() = default; - ControlAllocator(const ControlAllocator&) = default; - ControlAllocator(ControlAllocator&&) = default; - ControlAllocator& operator=(const ControlAllocator&) = default; - ControlAllocator& operator=(ControlAllocator&&) = default; - - // METHODS - void addControl(const std::string& controlName) { - const int index = m_controlIndexMap.size(); - m_controlIndexMap[controlName] = index; - } - - void setControls(SimTK::State& s, - const SimTK::Vector& controls) const { - updControls(s) = controls; - } - - SimTK::Vector& updControls(SimTK::State& s) const { - const SimTK::Subsystem& subSys = getSystem().getDefaultSubsystem(); - auto& dv = subSys.updDiscreteVariable(s, m_discreteVarIndex); - auto& discreteControls = - SimTK::Value::updDowncast(dv).upd(); - return discreteControls; - } - - const SimTK::Vector& getControls( - const SimTK::State& s) const { - const SimTK::Subsystem& subSys = getSystem().getDefaultSubsystem(); - auto& dv = subSys.getDiscreteVariable(s, m_discreteVarIndex); - auto& discreteControls = SimTK::Value::downcast(dv).get(); - return discreteControls; - } - - double getControlForOutputChannel(const SimTK::State& s, - const std::string& channel) const { - return getControls(s)[m_controlIndexMap.at(channel)]; - } - -protected: - void extendRealizeTopology(SimTK::State& state) const { - Super::extendRealizeTopology(state); - const SimTK::Subsystem& subSys = getSystem().getDefaultSubsystem(); - - const SimTK::Vector initControls( - static_cast(m_controlIndexMap.size()), 0.0); - - m_discreteVarIndex = - subSys.allocateDiscreteVariable(state, SimTK::Stage::Dynamics, - new SimTK::Value(initControls)); - } - - - void extendFinalizeFromProperties() { - Super::extendFinalizeFromProperties(); - for (const auto& kv : m_controlIndexMap) { - updOutput("controls").addChannel(kv.first); - } - } - -private: - std::unordered_map m_controlIndexMap; - mutable SimTK::DiscreteVariableIndex m_discreteVarIndex; - -}; - -// - -// class InputController : public Controller { -// OpenSim_DECLARE_ABSTRACT_OBJECT(InputController, Controller); -// public: -// OpenSim_DECLARE_LIST_INPUT(controls, double, SimTK::Stage::Dynamics, "controls"); -// InputController() = default; -// }; -// -// class ActuatorController : public InputController { -// OpenSim_DECLARE_CONCRETE_OBJECT(ActuatorController, InputController); -// public: -// ActuatorController() = default; -// -// void computeControls(const SimTK::State& s, -// SimTK::Vector& controls) const { -// -// SimTK::Vector control(1, 0.0); -// for (int i = 0; i < getActuatorSet().getSize(); ++i) { -// const auto& actu = getActuatorSet().get(i); -// const unsigned index = m_aliasMap.at(actu.getName()); -// control[0] = getInput("controls").getValue(s, index); -// actu.addInControls(control, controls); -// } -// -// } -// -// protected: -// void extendFinalizeFromProperties() { -// Super::extendFinalizeFromProperties(); -// const auto& input = getInput("controls"); -// for (int i = 0; i < input.getNumConnectees(); ++i) { -// const auto& alias = input.getAlias(i); -// OPENSIM_THROW_IF(!getActuatorSet().contains(alias), Exception, -// "Actuator '{}' not found in the ActuatorController's " -// "ActuatorSet.", alias); -// m_aliasMap[alias] = i; -// } -// } -// -// private: -// std::unordered_map m_aliasMap; -// -// }; int main() { @@ -172,8 +55,8 @@ int main() { SimTK::State state = model.initSystem(); SimTK::Vector newControls(2, 0.0); - newControls[0] = 1.0; - newControls[1] = 2.0; + newControls[0] = 1.23; + newControls[1] = 4.56; allocator->setControls(state, newControls); model.realizeDynamics(state); diff --git a/OpenSim/Simulation/Control/Controller.h b/OpenSim/Simulation/Control/Controller.h index 2587cedf30..8525fe21d9 100644 --- a/OpenSim/Simulation/Control/Controller.h +++ b/OpenSim/Simulation/Control/Controller.h @@ -31,16 +31,6 @@ namespace OpenSim { class Model; class Actuator; -class OSIMSIMULATION_API ControllerActuator : public Component { -OpenSim_DECLARE_CONCRETE_OBJECT(ControllerActuator, Component); - -public: - OpenSim_DECLARE_SOCKET(actuator, Actuator, - "The actuator that this controller controls."); - - ControllerActuator() = default; -}; - /** * Controller is an abstract ModelComponent that defines the interface for * an OpenSim Controller. A controller computes and sets the values of the diff --git a/OpenSim/Simulation/Control/InputController.cpp b/OpenSim/Simulation/Control/InputController.cpp index 4f85d0fedc..000bb82ce5 100644 --- a/OpenSim/Simulation/Control/InputController.cpp +++ b/OpenSim/Simulation/Control/InputController.cpp @@ -38,19 +38,10 @@ InputController::InputController() : Controller() {} InputController::~InputController() = default; -InputController::InputController(const InputController& other) - : Controller(other) {} - -InputController& InputController::operator=(const InputController& other) { - // Controller uses a custom copy constructor, so we need to explicitly - // call the base class assignment operator. - if (this != &other) { - Super::operator=(other); - m_controlNames = other.m_controlNames; - m_controlIndexes = other.m_controlIndexes; - } - return *this; -} +InputController::InputController(const InputController& other) = default; + +InputController& +InputController::operator=(const InputController& other) = default; InputController::InputController(InputController&& other) noexcept = default; @@ -70,7 +61,7 @@ const std::vector& InputController::getControlIndexes() const { std::vector InputController::getInputChannelAliases() const { std::vector aliases; - const auto& input = getInput("inputs"); + const auto& input = getInput("controls"); aliases.reserve(input.getNumConnectees()); for (int i = 0; i < input.getNumConnectees(); ++i) { aliases.push_back(input.getAlias(i)); @@ -137,19 +128,10 @@ ActuatorInputController::ActuatorInputController() : InputController() {} ActuatorInputController::~ActuatorInputController() = default; ActuatorInputController::ActuatorInputController( - const ActuatorInputController& other) : InputController(other) {} + const ActuatorInputController& other) = default; ActuatorInputController& ActuatorInputController::operator=( - const ActuatorInputController& other) { - // Controller uses a custom copy constructor, so we need to explicitly - // call the base class assignment operator. - if (this != &other) { - Super::operator=(other); - m_controlIndexesInConnecteeOrder = - other.m_controlIndexesInConnecteeOrder; - } - return *this; -} + const ActuatorInputController& other) = default; ActuatorInputController::ActuatorInputController( ActuatorInputController&& other) noexcept = default; @@ -162,7 +144,7 @@ ActuatorInputController& ActuatorInputController::operator=( //============================================================================= void ActuatorInputController::computeControls(const SimTK::State& s, SimTK::Vector& controls) const { - const auto& input = getInput("inputs"); + const auto& input = getInput("controls"); for (int i = 0; i < input.getNumConnectees(); ++i) { controls[m_controlIndexesInConnecteeOrder[i]] = input.getValue(s, i); } @@ -179,7 +161,7 @@ void ActuatorInputController::extendConnectToModel(Model& model) { OPENSIM_ASSERT(getNumControls() == static_cast(controlNames.size())) - const auto& input = getInput("inputs"); + const auto& input = getInput("controls"); OPENSIM_THROW_IF(input.getNumConnectees() != getNumControls(), Exception, "Expected the number of Input connectees ({}) to match the number " "of actuators controls ({}), but they do not.", diff --git a/OpenSim/Simulation/Control/InputController.h b/OpenSim/Simulation/Control/InputController.h index c6b42f534a..bd518156c5 100644 --- a/OpenSim/Simulation/Control/InputController.h +++ b/OpenSim/Simulation/Control/InputController.h @@ -62,7 +62,7 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(InputController, Controller); // INPUTS //============================================================================= OpenSim_DECLARE_LIST_INPUT( - controls, double, SimTK::Stage::Dynamics, "inputs"); + controls, double, SimTK::Stage::Dynamics, "TODO"); //============================================================================= // METHODS diff --git a/OpenSim/Simulation/SimulationUtilities.cpp b/OpenSim/Simulation/SimulationUtilities.cpp index 488a69dbff..e43eb3b8d3 100644 --- a/OpenSim/Simulation/SimulationUtilities.cpp +++ b/OpenSim/Simulation/SimulationUtilities.cpp @@ -307,52 +307,12 @@ std::unordered_map OpenSim::createSystemYIndexMap( } std::vector OpenSim::createControlledActuatorPathsFromModel( - const Model& model, const bool ignoreDiscreteController) { - std::vector actuPaths; - for (const auto& controller : model.getComponentList()) { - if (dynamic_cast(&controller) && - ignoreDiscreteController) { - continue; - } - const auto& socket = controller.getSocket("actuators"); - for (int i = 0; i < socket.getNumConnectees(); ++i) { - const auto& actu = socket.getConnectee(i); - const auto& actuPath = actu.getAbsolutePathString(); - if (std::find(actuPaths.begin(), actuPaths.end(), - actuPath) == actuPaths.end()) { - actuPaths.push_back(actuPath); - } - - } - } - - // Remove any actuators that are also controlled by a DiscreteController. - if (ignoreDiscreteController) { - for (const auto& controller : - model.getComponentList()) { - const auto& socket = controller.getSocket("actuators"); - for (int i = 0; i < socket.getNumConnectees(); ++i) { - const auto& actu = socket.getConnectee(i); - const auto& actuPath = actu.getAbsolutePathString(); - auto it = std::find(actuPaths.begin(), actuPaths.end(), - actuPath); - if (it != actuPaths.end()) { - actuPaths.erase(it); - } - } - } - } - - return actuPaths; + const Model& model) { } std::vector OpenSim::createControlNamesFromModel( - const Model& model, std::vector& modelControlIndices, - const bool skipControlledActuators, - const bool ignoreDiscreteController) { + const Model& model, std::vector& modelControlIndices) { std::vector controlNames; - std::vector controlledActuPaths = - createControlledActuatorPathsFromModel(model, ignoreDiscreteController); // Loop through all actuators and create control names. For scalar // actuators, use the actuator name for the control name. For non-scalar @@ -361,15 +321,10 @@ std::vector OpenSim::createControlNamesFromModel( int count = 0; modelControlIndices.clear(); for (const auto& actu : model.getComponentList()) { - const bool isControlledActuator = - std::find(controlledActuPaths.begin(), controlledActuPaths.end(), - actu.getAbsolutePathString()) != controlledActuPaths.end(); - if (!actu.get_appliesForce() || - (skipControlledActuators && isControlledActuator)) { + if (!actu.get_appliesForce()) { count += actu.numControls(); continue; } - std::string actuPath = actu.getAbsolutePathString(); if (actu.numControls() == 1) { controlNames.push_back(actuPath); @@ -388,17 +343,13 @@ std::vector OpenSim::createControlNamesFromModel( } std::vector OpenSim::createControlNamesFromModel( - const Model& model, - const bool skipControlledActuators, - const bool ignoreDiscreteController) { + const Model& model) { std::vector modelControlIndices; - return createControlNamesFromModel(model, modelControlIndices, - skipControlledActuators, ignoreDiscreteController); + return createControlNamesFromModel(model, modelControlIndices); } std::unordered_map OpenSim::createSystemControlIndexMap( - const Model& model, const bool skipControlledActuators, - const bool ignoreDiscreteController) { + const Model& model) { // We often assume that control indices in the state are in the same order // as the actuators in the model. However, the control indices are // allocated in the order in which addToSystem() is invoked (not @@ -407,8 +358,6 @@ std::unordered_map OpenSim::createSystemControlIndexMap( // we can run the following check: in order, set an actuator's control // signal(s) to NaN and ensure the i-th control is NaN. std::unordered_map controlIndices; - std::vector controlledActuPaths = - createControlledActuatorPathsFromModel(model, ignoreDiscreteController); const SimTK::State state = model.getWorkingState(); auto modelControls = model.updControls(state); int i = 0; @@ -419,17 +368,10 @@ std::unordered_map OpenSim::createSystemControlIndexMap( actu.getControls(modelControls, origControls); actu.setControls(nan, modelControls); std::string actuPath = actu.getAbsolutePathString(); - const bool isControlledActuator = - std::find(controlledActuPaths.begin(), controlledActuPaths.end(), - actu.getAbsolutePathString()) != controlledActuPaths.end(); for (int j = 0; j < nc; ++j) { OPENSIM_THROW_IF(!SimTK::isNaN(modelControls[i]), Exception, "Internal error: actuators are not in the " "expected order. Submit a bug report."); - if (skipControlledActuators && isControlledActuator) { - ++i; - continue; - } if (nc == 1) { controlIndices[actuPath] = i; } else { @@ -443,7 +385,7 @@ std::unordered_map OpenSim::createSystemControlIndexMap( } void OpenSim::checkOrderSystemControls(const Model& model) { - createSystemControlIndexMap(model, false, false); + createSystemControlIndexMap(model); } void OpenSim::checkLabelsMatchModelStates(const Model& model, diff --git a/OpenSim/Simulation/SimulationUtilities.h b/OpenSim/Simulation/SimulationUtilities.h index 06251b6f87..01334eb512 100644 --- a/OpenSim/Simulation/SimulationUtilities.h +++ b/OpenSim/Simulation/SimulationUtilities.h @@ -153,12 +153,9 @@ std::unordered_map createSystemYIndexMap(const Model& model); #endif /// Create a vector of actuator paths in the model associated with a Controller. -/// The flag 'ignoreDiscreteController' is used to exclude any actuators -/// associated a DiscreteController, which is used internally by Moco to pass -/// control variables from a solver to a model. OSIMSIMULATION_API std::vector createControlledActuatorPathsFromModel( - const Model& model, bool ignoreDiscreteController = false); + const Model& model); /// Create a vector of control names based on the actuators in the model for /// which appliesForce == True. For actuators with one control (e.g. @@ -168,28 +165,17 @@ std::vector createControlledActuatorPathsFromModel( /// to the number of controls associated with actuators that apply a force /// (appliesForce == True). Its elements are the indices of the controls in the /// Model::updControls() that are associated with actuators that apply a force. -/// Use 'skipControlledActuators' to exclude any actuators associated with a -/// Controller; 'ignoreDiscreteController' is used to ignore the logic -/// 'skipControlledActuators' for DiscreteController%s (see -/// createControlledActuatorPathsFromModel()). /// @ingroup simulationutil OSIMSIMULATION_API std::vector createControlNamesFromModel( - const Model& model, std::vector& modelControlIndices, - bool skipControlledActuators = false, - bool ignoreDiscreteController = false); + const Model& model, std::vector& modelControlIndices); /// Same as above, but when there is no mapping to the modelControlIndices. /// @ingroup simulationutil OSIMSIMULATION_API -std::vector createControlNamesFromModel(const Model& model, - bool skipControlledActuators = false, - bool ignoreDiscreteController = false); +std::vector createControlNamesFromModel(const Model& model); /// The map provides the index of each control variable in the SimTK::Vector /// returned by Model::getControls(), using the control name as the -/// key. Use 'skipControlledActuators' to exclude any actuators associated with -/// a Controller; 'ignoreDiscreteController' is used to ignore the logic -/// 'skipControlledActuators' for DiscreteController%s (see -/// createControlledActuatorPathsFromModel()). +/// key. /// /// @throws Exception if the order of actuators in the model does not match /// the order of controls in Model::getControls(). This is an internal @@ -198,9 +184,7 @@ std::vector createControlNamesFromModel(const Model& model, /// @ingroup simulationutil OSIMSIMULATION_API std::unordered_map createSystemControlIndexMap( - const Model& model, - bool skipControlledActuators = false, - bool ignoreDiscreteController = false); + const Model& model); /// Throws an exception if the order of the controls in the model is not the /// same as the order of the actuators in the model. From b5ba1bf03d1f4147478381faa8204c842a202fc8 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Thu, 25 Jan 2024 15:46:29 -0800 Subject: [PATCH 38/48] Update all goals after reverting SimulationUtilities --- .../Moco/MocoCasADiSolver/MocoCasOCProblem.h | 37 +++++++------- OpenSim/Moco/MocoControlBoundConstraint.cpp | 23 +++++++-- OpenSim/Moco/MocoControlBoundConstraint.h | 19 +++++-- OpenSim/Moco/MocoGoal/MocoControlGoal.cpp | 23 ++++++++- OpenSim/Moco/MocoGoal/MocoControlGoal.h | 23 +++++++-- .../Moco/MocoGoal/MocoControlTrackingGoal.cpp | 23 +++++++-- .../Moco/MocoGoal/MocoControlTrackingGoal.h | 19 ++++++- .../MocoGoal/MocoInitialActivationGoal.cpp | 16 +++--- .../Moco/MocoGoal/MocoInitialActivationGoal.h | 3 -- OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.cpp | 29 ++++++++++- OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.h | 17 +++++++ OpenSim/Moco/MocoProblem.cpp | 2 +- .../MocoUserControlCost.cpp | 4 +- OpenSim/Simulation/SimulationUtilities.cpp | 5 -- OpenSim/Simulation/SimulationUtilities.h | 51 +++++++++++++++++-- 15 files changed, 228 insertions(+), 66 deletions(-) diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h index 278492b4f9..d35207bdf0 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h @@ -21,9 +21,9 @@ #include "CasOCProblem.h" #include "MocoCasADiSolver.h" -#include #include #include +#include #include #include @@ -345,9 +345,9 @@ class MocoCasOCProblem : public CasOC::Problem { auto& simtkStateDisabledConstraints = mocoProblemRep->updStateDisabledConstraints(); - const auto& discreteController = - mocoProblemRep->getDiscreteControllerDisabledConstraints(); - const auto& rawControls = discreteController.getDiscreteControls( + const auto& modelDisabledConstraints = + mocoProblemRep->getModelDisabledConstraints(); + const auto& rawControls = modelDisabledConstraints.getControls( simtkStateDisabledConstraints); integrand = mocoCost.calcIntegrand( @@ -376,11 +376,11 @@ class MocoCasOCProblem : public CasOC::Problem { auto& simtkStateDisabledConstraintsFinal = mocoProblemRep->updStateDisabledConstraints(1); - const auto& discreteController = - mocoProblemRep->getDiscreteControllerDisabledConstraints(); - const auto& rawControlsInitial = discreteController.getDiscreteControls( + const auto& modelDisabledConstraints = + mocoProblemRep->getModelDisabledConstraints(); + const auto& rawControlsInitial = modelDisabledConstraints.getControls( simtkStateDisabledConstraintsInitial); - const auto& rawControlsFinal = discreteController.getDiscreteControls( + const auto& rawControlsFinal = modelDisabledConstraints.getControls( simtkStateDisabledConstraintsFinal); // Compute the cost for this cost term. @@ -410,9 +410,9 @@ class MocoCasOCProblem : public CasOC::Problem { auto& simtkStateDisabledConstraints = mocoProblemRep->updStateDisabledConstraints(); - const auto& discreteController = - mocoProblemRep->getDiscreteControllerDisabledConstraints(); - const auto& rawControls = discreteController.getDiscreteControls( + const auto& modelDisabledConstraints = + mocoProblemRep->getModelDisabledConstraints(); + const auto& rawControls = modelDisabledConstraints.getControls( simtkStateDisabledConstraints); integrand = mocoEC.calcIntegrand( @@ -442,11 +442,11 @@ class MocoCasOCProblem : public CasOC::Problem { auto& simtkStateDisabledConstraintsFinal = mocoProblemRep->updStateDisabledConstraints(1); - const auto& discreteController = - mocoProblemRep->getDiscreteControllerDisabledConstraints(); - const auto& rawControlsInitial = discreteController.getDiscreteControls( + const auto& modelDisabledConstraints = + mocoProblemRep->getModelDisabledConstraints(); + const auto& rawControlsInitial = modelDisabledConstraints.getControls( simtkStateDisabledConstraintsInitial); - const auto& rawControlsFinal = discreteController.getDiscreteControls( + const auto& rawControlsFinal = modelDisabledConstraints.getControls( simtkStateDisabledConstraintsFinal); // Compute the cost for this cost term. @@ -553,17 +553,16 @@ class MocoCasOCProblem : public CasOC::Problem { const double& time, const casadi::DM& states, const casadi::DM& controls, const Model& model, SimTK::State& simtkState, - const DiscreteController& discreteController) const { + const ControlAllocator& controlAllocator) const { if (stageDep >= SimTK::Stage::Model) { convertStatesToSimTKState( stageDep, time, states, model, simtkState, true); - // TODO replace with ControlAllocator // TODO set controls to ControlAllocator in problem order? the // controls will need to match the order of the controls // add to ControlAllocator SimTK::Vector& simtkControls = - discreteController.updDiscreteControls(simtkState); + controlAllocator.updControls(simtkState); for (int ic = 0; ic < getNumControls(); ++ic) { // simtkControls[m_modelControlIndices[ic]] = *(controls.ptr() + ic); simtkControls[ic] = *(controls.ptr() + ic); @@ -627,7 +626,7 @@ class MocoCasOCProblem : public CasOC::Problem { convertStatesControlsToSimTKState(stageDep, time, states, controls, modelDisabledConstraints, simtkStateDisabledConstraints, - mocoProblemRep->getDiscreteControllerDisabledConstraints()); + mocoProblemRep->getControlAllocatorDisabledConstraints()); // If enabled constraints exist in the model, compute constraint forces // based on Lagrange multipliers. This also updates the associated diff --git a/OpenSim/Moco/MocoControlBoundConstraint.cpp b/OpenSim/Moco/MocoControlBoundConstraint.cpp index ce022b37f6..dd2fa95dc6 100644 --- a/OpenSim/Moco/MocoControlBoundConstraint.cpp +++ b/OpenSim/Moco/MocoControlBoundConstraint.cpp @@ -22,6 +22,7 @@ #include #include +#include using namespace OpenSim; @@ -34,6 +35,7 @@ void MocoControlBoundConstraint::constructProperties() { constructProperty_lower_bound(); constructProperty_upper_bound(); constructProperty_equality_with_lower(false); + constructProperty_ignore_controlled_actuators(false); } void MocoControlBoundConstraint::initializeOnModelImpl( @@ -42,6 +44,10 @@ void MocoControlBoundConstraint::initializeOnModelImpl( // Check that the model controls are in the correct order. checkOrderSystemControls(model); + // Get controls associated with the model's ActuatorInputController. + auto actuatorInputControls = + createControlNamesForControllerType(model); + m_hasLower = !getProperty_lower_bound().empty(); m_hasUpper = !getProperty_upper_bound().empty(); if (getProperty_control_paths().size() && !m_hasLower && !m_hasUpper) { @@ -50,8 +56,7 @@ void MocoControlBoundConstraint::initializeOnModelImpl( } // Make sure there are no nonexistent controls. if (m_hasLower || m_hasUpper) { - auto systemControlIndexMap = - createSystemControlIndexMap(model, true, true); + auto systemControlIndexMap = createSystemControlIndexMap(model); for (int i = 0; i < getProperty_control_paths().size(); ++i) { const auto& thisName = get_control_paths(i); OPENSIM_THROW_IF_FRMOBJ(systemControlIndexMap.count(thisName) == 0, @@ -59,7 +64,19 @@ void MocoControlBoundConstraint::initializeOnModelImpl( "Control path '{}' was provided but no such " "control exists in the model or it is already controlled " "by a user-defined controller.", - thisName); + thisName) + + bool isActuatorInputControl = std::find( + actuatorInputControls.begin(), actuatorInputControls.end(), + thisName) != actuatorInputControls.end(); + if (getIgnoreControlledActuators() && !isActuatorInputControl) { + log_info("MocoControlBoundConstraint: Control path '{}' is " + "already controlled by a user-defined controller " + "and will be ignored.", + thisName); + continue; + } + m_controlIndices.push_back(systemControlIndexMap[thisName]); } } diff --git a/OpenSim/Moco/MocoControlBoundConstraint.h b/OpenSim/Moco/MocoControlBoundConstraint.h index a24a00bc9e..c82b7242d9 100644 --- a/OpenSim/Moco/MocoControlBoundConstraint.h +++ b/OpenSim/Moco/MocoControlBoundConstraint.h @@ -34,14 +34,14 @@ If a function is a GCVSpline, we ensure that the spline covers the entire possible time range in the problem (using the problem's time bounds). We do not perform such a check for other types of functions. +If you wish to constrain all control signals except those belonging to a +user-defined controller, pass 'true' to setIgnoreControlledActuators(). + @note If you omit the lower and upper bounds, then this class will not constrain any control signals, even if you have provided control paths. @note This class can only constrain control signals for ScalarActuator%s. -@note Controls belonging to actuators controlled by user-defined controllers -will not appear in the MocoProblem, and therefore cannot be constrained by -this class. @ingroup mocopathcon */ class OSIMMOCO_API MocoControlBoundConstraint : public MocoPathConstraint { OpenSim_DECLARE_CONCRETE_OBJECT( @@ -88,6 +88,16 @@ class OSIMMOCO_API MocoControlBoundConstraint : public MocoPathConstraint { //// @copydoc setEqualityWithLower() bool getEqualityWithLower() const { return get_equality_with_lower(); } + /// If true, do not constrain controls belonging to actuators controlled by + /// user-defined controllers. + void setIgnoreControlledActuators(bool v) { + set_ignore_controlled_actuators(v); + } + /// @copydoc setIgnoreControlledActuators() + bool getIgnoreControlledActuators() const { + return get_ignore_controlled_actuators(); + } + protected: void initializeOnModelImpl( const Model& model, const MocoProblemInfo&) const override; @@ -106,6 +116,9 @@ class OSIMMOCO_API MocoControlBoundConstraint : public MocoPathConstraint { OpenSim_DECLARE_PROPERTY(equality_with_lower, bool, "The control must be equal to the lower bound; " "upper must be unspecified (default: false)."); + OpenSim_DECLARE_PROPERTY(ignore_controlled_actuators, bool, + "If true, do not constrain controls belonging to actuators " + "controlled by user-defined controllers (default: false)."); void constructProperties(); diff --git a/OpenSim/Moco/MocoGoal/MocoControlGoal.cpp b/OpenSim/Moco/MocoGoal/MocoControlGoal.cpp index aa3f69e95b..7b80c234cb 100644 --- a/OpenSim/Moco/MocoGoal/MocoControlGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoControlGoal.cpp @@ -20,6 +20,7 @@ #include #include +#include using namespace OpenSim; @@ -29,6 +30,7 @@ void MocoControlGoal::constructProperties() { constructProperty_control_weights(MocoWeightSet()); constructProperty_control_weights_pattern(MocoWeightSet()); constructProperty_exponent(2); + constructProperty_ignore_controlled_actuators(false); } void MocoControlGoal::setWeightForControl( @@ -52,12 +54,16 @@ void MocoControlGoal::setWeightForControlPattern( void MocoControlGoal::initializeOnModelImpl(const Model& model) const { // Get all expected control names. - auto controlNames = createControlNamesFromModel(model, true, true); + auto controlNames = createControlNamesFromModel(model); // Check that the model controls are in the correct order. checkOrderSystemControls(model); - auto systemControlIndexMap = createSystemControlIndexMap(model, true, true); + // Get controls associated with the model's ActuatorInputController. + auto actuatorInputControls = + createControlNamesForControllerType(model); + + auto systemControlIndexMap = createSystemControlIndexMap(model); // Make sure there are no weights for nonexistent controls. for (int i = 0; i < get_control_weights().getSize(); ++i) { const auto& thisName = get_control_weights()[i].getName(); @@ -90,10 +96,23 @@ void MocoControlGoal::initializeOnModelImpl(const Model& model) const { weight = weightsFromPatterns[controlName]; } + bool isActuatorInputControl = std::find( + actuatorInputControls.begin(), actuatorInputControls.end(), + controlName) != actuatorInputControls.end(); + if (getIgnoreControlledActuators() && !isActuatorInputControl) { + log_info("MocoControlGoal: Control '{}' is already controlled by a " + "user-defined controller and will be ignored.", + controlName); + continue; + } + if (weight != 0.0) { m_controlIndices.push_back(systemControlIndexMap[controlName]); m_weights.push_back(weight); m_controlNames.push_back(controlName); + } else { + log_info("MocoControlGoal: Control '{}' has weight 0 and will be " + "ignored.", controlName); } } diff --git a/OpenSim/Moco/MocoGoal/MocoControlGoal.h b/OpenSim/Moco/MocoGoal/MocoControlGoal.h index 3949dd6508..ea4bef4667 100644 --- a/OpenSim/Moco/MocoGoal/MocoControlGoal.h +++ b/OpenSim/Moco/MocoGoal/MocoControlGoal.h @@ -50,8 +50,8 @@ We use the following notation: If `p > 2`, we first take the absolute value of the control; this is to properly handle odd exponents. -@note Controls belonging to actuators controlled by user-defined controllers -are not included in the cost. +If you wish to minimize all control signals except those belonging to a +user-defined controller, pass 'true' to setIgnoreControlledActuators(). @ingroup mocogoal */ class OSIMMOCO_API MocoControlGoal : public MocoGoal { @@ -90,6 +90,16 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MocoControlGoal, MocoGoal); void setExponent(int exponent) { set_exponent(exponent); } double getExponent() const { return get_exponent(); } + /// If true, do not minimize controls belonging to actuators controlled by + /// user-defined controllers. + void setIgnoreControlledActuators(bool v) { + set_ignore_controlled_actuators(v); + } + /// @copydoc setIgnoreControlledActuators() + bool getIgnoreControlledActuators() const { + return get_ignore_controlled_actuators(); + } + protected: void initializeOnModelImpl(const Model&) const override; void calcIntegrandImpl( @@ -106,9 +116,12 @@ OpenSim_DECLARE_CONCRETE_OBJECT(MocoControlGoal, MocoGoal); OpenSim_DECLARE_PROPERTY(control_weights_pattern, MocoWeightSet, "Set control weights for all controls matching a regular " "expression."); - OpenSim_DECLARE_PROPERTY( - exponent, int, "The exponent on controls; greater than or equal to " - "2 (default: 2)."); + OpenSim_DECLARE_PROPERTY(exponent, int, + "The exponent on controls; greater than or equal to 2 " + "(default: 2)."); + OpenSim_DECLARE_PROPERTY(ignore_controlled_actuators, bool, + "If true, do not minimize controls belonging to actuators " + "controlled by user-defined controllers (default: false)."); mutable std::vector m_weights; mutable std::vector m_controlIndices; mutable std::vector m_controlNames; diff --git a/OpenSim/Moco/MocoGoal/MocoControlTrackingGoal.cpp b/OpenSim/Moco/MocoGoal/MocoControlTrackingGoal.cpp index 603c1f574e..c2c742fb6d 100644 --- a/OpenSim/Moco/MocoGoal/MocoControlTrackingGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoControlTrackingGoal.cpp @@ -22,6 +22,7 @@ #include #include +#include using namespace OpenSim; @@ -66,10 +67,12 @@ void MocoControlTrackingGoal::addScaleFactor(const std::string &name, void MocoControlTrackingGoal::initializeOnModelImpl(const Model& model) const { // Get a map between control names and their indices in the model. This also - // checks that the model controls are in the correct order. Controls - // associated with actuators controlled by user-defined controllers are not - // included in this map. - auto allControlIndices = createSystemControlIndexMap(model, true, true); + // checks that the model controls are in the correct order. + auto allControlIndices = createSystemControlIndexMap(model); + + // Get controls associated with the model's ActuatorInputController. + auto actuatorInputControls = + createControlNamesForControllerType(model); // Throw exception if a weight is specified for a nonexistent control. for (int i = 0; i < get_control_weights().getSize(); ++i) { @@ -152,6 +155,18 @@ void MocoControlTrackingGoal::initializeOnModelImpl(const Model& model) const { weight = get_control_weights().get(controlToTrack).getWeight(); } if (weight == 0) { + log_info("MocoControlTrackingGoal: Skipping control '{}' because " + "its weight is 0.", controlToTrack); + continue; + } + + bool isActuatorInputControl = std::find( + actuatorInputControls.begin(), actuatorInputControls.end(), + controlToTrack) != actuatorInputControls.end(); + if (getIgnoreControlledActuators() && !isActuatorInputControl) { + log_info("MocoControlTrackingGoal: Control '{}' is already " + "controlled by a user-defined controller and will be " + "ignored.", controlToTrack); continue; } diff --git a/OpenSim/Moco/MocoGoal/MocoControlTrackingGoal.h b/OpenSim/Moco/MocoGoal/MocoControlTrackingGoal.h index 392093a28d..f3b737dd98 100644 --- a/OpenSim/Moco/MocoGoal/MocoControlTrackingGoal.h +++ b/OpenSim/Moco/MocoGoal/MocoControlTrackingGoal.h @@ -131,8 +131,8 @@ Tracking problems in direct collocation perform best when tracking smooth data, so it is recommended to filter the data in the reference you provide to the cost. -@note Controls belonging to actuators controlled by user-defined controllers -cannot be tracked by this goal. +If you wish to track all control signals except those belonging to a +user-defined controller, pass 'true' to `setIgnoreControlledActuators()`. @ingroup mocogoal */ class OSIMMOCO_API MocoControlTrackingGoal : public MocoGoal { @@ -235,6 +235,16 @@ class OSIMMOCO_API MocoControlTrackingGoal : public MocoGoal { return get_allow_unused_references(); } + /// If true, do not track controls belonging to actuators controlled by + /// user-defined controllers. + void setIgnoreControlledActuators(bool v) { + set_ignore_controlled_actuators(v); + } + /// @copydoc setIgnoreControlledActuators() + bool getIgnoreControlledActuators() const { + return get_ignore_controlled_actuators(); + } + /// Add a MocoParameter to the problem that will scale the tracking reference /// data associated with the specified control. Scale factors are applied /// to the tracking error calculations based on the following equation: @@ -277,11 +287,16 @@ class OSIMMOCO_API MocoControlTrackingGoal : public MocoGoal { MocoControlTrackingGoalReference, "Association between controls and columns in the reference data."); + OpenSim_DECLARE_PROPERTY(ignore_controlled_actuators, bool, + "If true, do not track controls belonging to actuators " + "controlled by user-defined controllers (default: false)."); + void constructProperties() { constructProperty_reference(TableProcessor()); constructProperty_allow_unused_references(false); constructProperty_control_weights(MocoWeightSet()); constructProperty_reference_labels(); + constructProperty_ignore_controlled_actuators(false); } mutable std::vector m_control_indices; diff --git a/OpenSim/Moco/MocoGoal/MocoInitialActivationGoal.cpp b/OpenSim/Moco/MocoGoal/MocoInitialActivationGoal.cpp index 113f2b40cd..dccca44bf4 100644 --- a/OpenSim/Moco/MocoGoal/MocoInitialActivationGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoInitialActivationGoal.cpp @@ -19,6 +19,7 @@ #include "MocoInitialActivationGoal.h" #include +#include using namespace OpenSim; @@ -28,19 +29,14 @@ void MocoInitialActivationGoal::initializeOnModelImpl( // Get a map of all the state indices in the system. auto allSysYIndices = createSystemYIndexMap(model); - // Get a map of all the control indices in the system. Skip muscles that - // are already controlled by a user-defined controller. - auto systemControlIndexMap = createSystemControlIndexMap(model, true, true); + // Get a map of all the control indices in the system. + auto systemControlIndexMap = createSystemControlIndexMap(model); for (const auto& muscle : model.getComponentList()) { const std::string path = muscle.getAbsolutePathString(); - const bool muscleIsNotControlled = systemControlIndexMap.find(path) != - systemControlIndexMap.end(); - if (!muscle.get_ignore_activation_dynamics() && muscleIsNotControlled) { - int excitationIndex = systemControlIndexMap[path]; - int activationIndex = allSysYIndices[path + "/activation"]; - m_indices.emplace_back(excitationIndex, activationIndex); - } + int excitationIndex = systemControlIndexMap[path]; + int activationIndex = allSysYIndices[path + "/activation"]; + m_indices.emplace_back(excitationIndex, activationIndex); } setRequirements(0, (int)m_indices.size(), SimTK::Stage::Time); diff --git a/OpenSim/Moco/MocoGoal/MocoInitialActivationGoal.h b/OpenSim/Moco/MocoGoal/MocoInitialActivationGoal.h index b7380df71b..5f6fd30b01 100644 --- a/OpenSim/Moco/MocoGoal/MocoInitialActivationGoal.h +++ b/OpenSim/Moco/MocoGoal/MocoInitialActivationGoal.h @@ -31,9 +31,6 @@ This is an endpoint constraint goal by default. Credit for using this goal to address excessive initial activation goes to Jessica Allen. -@note Muscles that are already controlled by a user-defined controller are -ignored by this goal. - @ingroup mocogoal */ class OSIMMOCO_API MocoInitialActivationGoal : public MocoGoal { OpenSim_DECLARE_CONCRETE_OBJECT(MocoInitialActivationGoal, MocoGoal); diff --git a/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.cpp b/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.cpp index 0c1acfddf0..f815899f18 100644 --- a/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.cpp @@ -19,6 +19,7 @@ #include "MocoPeriodicityGoal.h" #include +#include using namespace OpenSim; @@ -78,16 +79,40 @@ void MocoPeriodicityGoal::initializeOnModelImpl(const Model& model) const { get_state_pairs(i).get_negate() ? -1 : 1); } - auto systemControlIndexMap = createSystemControlIndexMap(model, true, true); - int nControlPairs = getProperty_control_pairs().size(); + auto systemControlIndexMap = createSystemControlIndexMap(model); + + // Get controls associated with the model's ActuatorInputController. + auto actuatorInputControls = + createControlNamesForControllerType(model); + int nControlPairs = getProperty_control_pairs().size(); + bool pairHasActuatorInputControl; for (int i = 0; i < nControlPairs; ++i) { const auto path1 = get_control_pairs(i).get_initial_variable(); OPENSIM_THROW_IF(systemControlIndexMap.count(path1) == 0, Exception, "Could not find control variable '{}'.", path1); + pairHasActuatorInputControl = std::find( + actuatorInputControls.begin(), actuatorInputControls.end(), + path1) != actuatorInputControls.end(); + const auto path2 = get_control_pairs(i).get_final_variable(); OPENSIM_THROW_IF(systemControlIndexMap.count(path2) == 0, Exception, "Could not find control variable '{}'.", path2); + if (!pairHasActuatorInputControl) { + pairHasActuatorInputControl = std::find( + actuatorInputControls.begin(), actuatorInputControls.end(), + path2) != actuatorInputControls.end(); + } + + if (getIgnoreControlledActuators() && !pairHasActuatorInputControl) { + log_info("MocoPeriodicityGoal: Control pair '{}'/'{}' has at least " + "control that is already controlled by a user-defined " + "controller; therefore it will be ignored.", + get_control_pairs(i).get_initial_variable(), + get_control_pairs(i).get_final_variable()); + continue; + } + int controlIndex1 = systemControlIndexMap[path1]; int controlIndex2 = systemControlIndexMap[path2]; m_control_names.emplace_back(path1, path2); diff --git a/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.h b/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.h index dbb39c1b78..2ece9028f9 100644 --- a/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.h +++ b/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.h @@ -60,6 +60,10 @@ To handle initial and final variable values that are equal in absolute value but differ in sign (e.g. a pelvis rotation in walking), use addNegatedStatePair or addNegatedControlPair. +If you wish to constrain all control signal pairs except those containing a +control that belongs to a user-defined controller, pass 'true' to +setIgnoreControlledActuators(). + To impose bilateral symmetry in a walking simulation, we can simulate over half a gait cycle and impose periodic constraints. For bilateral variables (e.g., hip flexion speeds and hamstrings controls), the @@ -124,6 +128,16 @@ class OSIMMOCO_API MocoPeriodicityGoal : public MocoGoal { append_control_pairs(std::move(pair)); } + /// If true, do not constrain controls belonging to actuators controlled by + /// user-defined controllers. + void setIgnoreControlledActuators(bool v) { + set_ignore_controlled_actuators(v); + } + /// @copydoc setIgnoreControlledActuators() + bool getIgnoreControlledActuators() const { + return get_ignore_controlled_actuators(); + } + protected: bool getSupportsEndpointConstraintImpl() const override { return true; } Mode getDefaultModeImpl() const override { @@ -139,6 +153,9 @@ class OSIMMOCO_API MocoPeriodicityGoal : public MocoGoal { "Periodic pairs of states."); OpenSim_DECLARE_LIST_PROPERTY(control_pairs, MocoPeriodicityGoalPair, "Periodic pairs of controls."); + OpenSim_DECLARE_PROPERTY(ignore_controlled_actuators, bool, + "If true, do not constrain controls belonging to actuators " + "controlled by user-defined controllers (default: false)."); void constructProperties(); mutable std::vector> m_indices_states; mutable std::vector> m_indices_controls; diff --git a/OpenSim/Moco/MocoProblem.cpp b/OpenSim/Moco/MocoProblem.cpp index a72070735f..c3503f9783 100644 --- a/OpenSim/Moco/MocoProblem.cpp +++ b/OpenSim/Moco/MocoProblem.cpp @@ -108,7 +108,7 @@ void MocoPhase::printControlNamesWithSubstring(const std::string& substring) { std::vector foundNames; Model model = get_model().process(); model.initSystem(); - const auto controlNames = createControlNamesFromModel(model, true, true); + const auto controlNames = createControlNamesFromModel(model); for (int i = 0; i < (int)controlNames.size(); ++i) { if (controlNames[i].find(substring) != std::string::npos) { foundNames.push_back(controlNames[i]); diff --git a/OpenSim/Sandbox/Moco/sandboxMocoUserControlCost/MocoUserControlCost.cpp b/OpenSim/Sandbox/Moco/sandboxMocoUserControlCost/MocoUserControlCost.cpp index 1eb5b89780..b89523fda2 100644 --- a/OpenSim/Sandbox/Moco/sandboxMocoUserControlCost/MocoUserControlCost.cpp +++ b/OpenSim/Sandbox/Moco/sandboxMocoUserControlCost/MocoUserControlCost.cpp @@ -45,12 +45,12 @@ void MocoUserControlCost::setWeight( void MocoUserControlCost::initializeOnModelImpl(const Model& model) const { // Get all expected control names. - auto controlNames = createControlNamesFromModel(model, true, true); + auto controlNames = createControlNamesFromModel(model); // Check that the model controls are in the correct order. checkOrderSystemControls(model); - auto systemControlIndexMap = createSystemControlIndexMap(model, true, true); + auto systemControlIndexMap = createSystemControlIndexMap(model); // Make sure there are no weights for nonexistent controls. for (int i = 0; i < get_control_weights().getSize(); ++i) { const auto& thisName = get_control_weights()[i].getName(); diff --git a/OpenSim/Simulation/SimulationUtilities.cpp b/OpenSim/Simulation/SimulationUtilities.cpp index e43eb3b8d3..6b0adda007 100644 --- a/OpenSim/Simulation/SimulationUtilities.cpp +++ b/OpenSim/Simulation/SimulationUtilities.cpp @@ -32,7 +32,6 @@ #include #include #include -#include using namespace OpenSim; @@ -306,10 +305,6 @@ std::unordered_map OpenSim::createSystemYIndexMap( return sysYIndices; } -std::vector OpenSim::createControlledActuatorPathsFromModel( - const Model& model) { -} - std::vector OpenSim::createControlNamesFromModel( const Model& model, std::vector& modelControlIndices) { std::vector controlNames; diff --git a/OpenSim/Simulation/SimulationUtilities.h b/OpenSim/Simulation/SimulationUtilities.h index 01334eb512..59db7b03e5 100644 --- a/OpenSim/Simulation/SimulationUtilities.h +++ b/OpenSim/Simulation/SimulationUtilities.h @@ -152,11 +152,6 @@ OSIMSIMULATION_API std::unordered_map createSystemYIndexMap(const Model& model); #endif -/// Create a vector of actuator paths in the model associated with a Controller. -OSIMSIMULATION_API -std::vector createControlledActuatorPathsFromModel( - const Model& model); - /// Create a vector of control names based on the actuators in the model for /// which appliesForce == True. For actuators with one control (e.g. /// ScalarActuator) the control name is simply the actuator name. For actuators @@ -169,10 +164,56 @@ std::vector createControlledActuatorPathsFromModel( OSIMSIMULATION_API std::vector createControlNamesFromModel( const Model& model, std::vector& modelControlIndices); + /// Same as above, but when there is no mapping to the modelControlIndices. /// @ingroup simulationutil OSIMSIMULATION_API std::vector createControlNamesFromModel(const Model& model); + +/// Given a controller of type `T`, create a vector of control names based on +/// the actuators in the model for which appliesForce == True. For actuators +/// with one control (e.g. ScalarActuator) the control name is simply the +/// actuator name. For actuators with multiple controls, each control name is +/// the actuator name appended by the control index (e.g. "/actuator_0"). +/// @ingroup simulationutil +template +std::vector createControlNamesForControllerType( + const Model& model) { + std::vector controlNames; + + // Check that T is a Controller. + if (!std::is_base_of::value) { + std::stringstream msg; + msg << "Expected the template argument to derived from Controller, " + << "but the provided type is " << typeid(T).name() << "."; + OPENSIM_THROW(Exception, msg.str()); + } + + for (const auto& controller : model.getComponentList()) { + const auto& socket = controller.getSocket("actuators"); + // Loop through all actuators and create control names. For scalar + // actuators, use the actuator name for the control name. For non-scalar + // actuators, use the actuator name with a control index appended for + // the control name. + for (int i = 0; i < socket.getNumConnectees(); ++i) { + const auto& actu = socket.getConnectee(i); + if (!actu.get_appliesForce()) { + continue; + } + std::string actuPath = actu.getAbsolutePathString(); + if (actu.numControls() == 1) { + controlNames.push_back(actuPath); + } else { + for (int i = 0; i < actu.numControls(); ++i) { + controlNames.push_back(actuPath + "_" + std::to_string(i)); + } + } + } + } + + return controlNames; +} + /// The map provides the index of each control variable in the SimTK::Vector /// returned by Model::getControls(), using the control name as the /// key. From b40bed149e36ab51afc93f2276362edf66b29300 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Fri, 26 Jan 2024 12:26:12 -0800 Subject: [PATCH 39/48] Wiring everything together --- OpenSim/Moco/Components/ControlAllocator.cpp | 14 ++++ OpenSim/Moco/Components/ControlAllocator.h | 6 ++ .../MocoCasADiSolver/MocoCasOCProblem.cpp | 12 +-- .../Moco/MocoCasADiSolver/MocoCasOCProblem.h | 6 +- OpenSim/Moco/MocoProblemRep.cpp | 79 +++++++++++++------ OpenSim/Moco/tropter/TropterProblem.h | 31 +++----- 6 files changed, 95 insertions(+), 53 deletions(-) diff --git a/OpenSim/Moco/Components/ControlAllocator.cpp b/OpenSim/Moco/Components/ControlAllocator.cpp index fa140e542d..ceda09abf8 100644 --- a/OpenSim/Moco/Components/ControlAllocator.cpp +++ b/OpenSim/Moco/Components/ControlAllocator.cpp @@ -69,6 +69,20 @@ double ControlAllocator::getControlForOutputChannel(const SimTK::State& s, return getControls(s)[m_controlIndexMap.at(channel)]; } +std::vector ControlAllocator::getControlNamesInOrder() const { + // Return the control names in ascending order of their indices. + std::vector 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 //============================================================================= diff --git a/OpenSim/Moco/Components/ControlAllocator.h b/OpenSim/Moco/Components/ControlAllocator.h index 10aadf373f..87979ee7d3 100644 --- a/OpenSim/Moco/Components/ControlAllocator.h +++ b/OpenSim/Moco/Components/ControlAllocator.h @@ -79,6 +79,12 @@ class OSIMMOCO_API ControlAllocator : public ModelComponent { 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 + * allocator. + */ + std::vector getControlNamesInOrder() const; + protected: // MODEL COMPONENT INTERFACE void extendRealizeTopology(SimTK::State& state) const override; diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp index 4fd4913519..e6d2795340 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp @@ -63,15 +63,11 @@ MocoCasOCProblem::MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver, convertBounds(info.getFinalBounds())); } - // Add control variables to the problem (i.e., any controls for actuators - // in the model's DiscreteController). First, double that the system - // controls are in the same order as the model. - checkOrderSystemControls(model); - // Get the list of control names (based on actuator paths) in the order of - // the system controls, which is the order the DiscreteController expects. - // Add the controls to the problem in the same order. - auto controlNames = createControlNamesFromModel(model, true, true); + auto controlNames = + problemRep.getControlAllocatorBase().getControlNamesInOrder(); for (const auto& controlName : controlNames) { + // TODO will need to check for "input" infos once we support user + // InputControllers. const auto& info = problemRep.getControlInfo(controlName); addControl(controlName, convertBounds(info.getBounds()), convertBounds(info.getInitialBounds()), diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h index d35207bdf0..695da0b54c 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h @@ -558,13 +558,9 @@ class MocoCasOCProblem : public CasOC::Problem { convertStatesToSimTKState( stageDep, time, states, model, simtkState, true); - // TODO set controls to ControlAllocator in problem order? the - // controls will need to match the order of the controls - // add to ControlAllocator SimTK::Vector& simtkControls = controlAllocator.updControls(simtkState); for (int ic = 0; ic < getNumControls(); ++ic) { - // simtkControls[m_modelControlIndices[ic]] = *(controls.ptr() + ic); simtkControls[ic] = *(controls.ptr() + ic); } } @@ -754,7 +750,7 @@ class MocoCasOCProblem : public CasOC::Problem { bool m_paramsRequireInitSystem = true; std::string m_formattedTimeString; std::unordered_map m_yIndexMap; - std::vector m_modelControlIndices; + //std::vector m_modelControlIndices; std::unique_ptr m_fileDeletionThrower; // Local memory to hold constraint forces. static thread_local SimTK::Vector_ diff --git a/OpenSim/Moco/MocoProblemRep.cpp b/OpenSim/Moco/MocoProblemRep.cpp index 47a00b5864..65f633046e 100644 --- a/OpenSim/Moco/MocoProblemRep.cpp +++ b/OpenSim/Moco/MocoProblemRep.cpp @@ -102,16 +102,8 @@ void MocoProblemRep::initialize() { // the actuators in the model. checkOrderSystemControls(m_model_base); - // Steps: - // 1) Check that existing controllers are valid (PrescribedController, for now) - // 2) Skip over actuators that have controllers (when the user does not want to add controls for these actuators) - // 3) Add a ControlAllocator - - // TODO need a MocoProblem option to allow adding OCP controls for - // that already have a controller. - - // Check that the any controllers added by the user are valid. - std::vector controlNamesToAddToControlAllocator; // TODO better name + // Check that any controllers added by the user are valid. + std::vector controlledActuatorPaths; for (const auto& controller : m_model_base.getComponentList()) { if (!dynamic_cast(&controller)) { OPENSIM_THROW(Exception, "Moco only supports PrescribedController " @@ -120,14 +112,12 @@ void MocoProblemRep::initialize() { controller.getAbsolutePathString(), controller.getConcreteClassName()); } + const auto& socket = controller.getSocket("actuators"); + for (int i = 0; i < socket.getNumConnectees(); ++i) { + controlledActuatorPaths.push_back(socket.getConnecteePath(i)); + } } - // Get the list of actuators that are controlled by the user. There should - // be no DiscreteController in the model at this point, so we set the second - // argument to false. - std::vector controlledActuatorPaths = - createControlledActuatorPathsFromModel(m_model_base, false); - // Add the non-controlled, enabled actuators to an ActuatorInputController. auto actuatorController = make_unique(); actuatorController->setName("actuator_controller"); @@ -142,10 +132,43 @@ void MocoProblemRep::initialize() { } m_model_base.addController(actuatorController.release()); - // Add a ControlAllocator to the model. + // Add a ControlAllocator to the model. This component will distribute the + // OCP controls to all InputControllers in the model, including the + // ActuatorInputController we just added to control actuators that are not + // already controlled by a user-defined controller. + auto inputControllerNames = + createControlNamesForControllerType(m_model_base); auto controlAllocatorUPtr = make_unique(); + for (const auto& controlName : inputControllerNames) { + controlAllocatorUPtr->addControl(controlName); + } m_control_allocator_base.reset(controlAllocatorUPtr.get()); m_model_base.addComponent(controlAllocatorUPtr.release()); + m_model_base.finalizeFromProperties(); + + // Wire the ControlAllocators to the InputControllers in the model. + for (auto& controller : m_model_base.updComponentList()) { + const auto& socket = controller.getSocket("actuators"); + for (int i = 0; i < socket.getNumConnectees(); ++i) { + const auto& actu = socket.getConnectee(i); + const auto& actuPath = actu.getAbsolutePathString(); + if (actu.numControls() > 1) { + const auto& channel = + m_control_allocator_base->getOutput("controls") + .getChannel(actuPath); + controller.connectInput_controls(channel, actuPath); + } else { + for (int ic = 0; ic < actu.numControls(); ++ic) { + std::string controlName = + actuPath + "_" + std::to_string(ic); + const auto& channel = + m_control_allocator_base->getOutput("controls") + .getChannel(controlName); + controller.connectInput_controls(channel, controlName); + } + } + } + } // Scale factors // ------------- @@ -477,8 +500,18 @@ void MocoProblemRep::initialize() { // Control infos. // -------------- - std::vector controlNames = createControlNamesFromModel( - m_model_base, true, true); + // TODO wait, this looks wrong... + auto controlNames = createControlNamesFromModel(m_model_base); + auto actuatorInputControlNames = + createControlNamesForControllerType( + m_model_base); + // Remove the actuators controlled by an ActuatorInputController from + // controlNames. TODO wait, shouldn't these be the only ones to check...? + for (const auto& name : actuatorInputControlNames) { + controlNames.erase(std::remove(controlNames.begin(), + controlNames.end(), name), controlNames.end()); + } + for (int i = 0; i < ph0.getProperty_control_infos_pattern().size(); ++i) { const auto& pattern = ph0.get_control_infos_pattern(i).getName(); auto regexPattern = std::regex(pattern); @@ -513,6 +546,9 @@ void MocoProblemRep::initialize() { m_model_base.getComponent( "actuator_controller"); const auto& socket = actuController.getSocket("actuators"); + + // TODO use createControlNamesForControllerType() here? Then + // do we disallow user added ActuatorInputControllers? for (int i = 0; i < socket.getNumConnectees(); ++i) { const auto& actu = socket.getConnectee(i); const std::string actuName = actu.getAbsolutePathString(); @@ -553,9 +589,8 @@ void MocoProblemRep::initialize() { } } - // Bound activations based on muscle excitation control bounds bounds. Set - // this for all muscles, including those controlled by a user-defined - // controller. + // Bound activations based on muscle excitation control bounds. Set this for + // all muscles, including those controlled by a user-defined controller. if (ph0.get_bound_activation_from_excitation()) { for (const auto& actu : m_model_base.getComponentList()) { const std::string actuName = actu.getAbsolutePathString(); diff --git a/OpenSim/Moco/tropter/TropterProblem.h b/OpenSim/Moco/tropter/TropterProblem.h index 898c487d1a..e9f19495bc 100644 --- a/OpenSim/Moco/tropter/TropterProblem.h +++ b/OpenSim/Moco/tropter/TropterProblem.h @@ -22,12 +22,12 @@ #include #include +#include #include #include #include #include #include -#include #include @@ -95,16 +95,11 @@ class MocoTropterSolver::TropterProblemBase : public tropter::Problem { } void addControlVariables() { - // Add control variables to the problem (i.e., any controls for - // actuators in the model's DiscreteController). First, double check - // that the system controls are in the same order as the model. - checkOrderSystemControls(m_modelBase); - // Get the list of control names (based on actuator paths) in the order - // of the system controls, which is the order the DiscreteController - // expects. Add the controls to the problem in the same order. auto controlNames = - createControlNamesFromModel(m_modelBase, true, true); + m_mocoProbRep.getControlAllocatorBase().getControlNamesInOrder(); for (const auto& controlName : controlNames) { + // TODO will need to check for "input" infos once we support user + // InputControllers. const auto& info = m_mocoProbRep.getControlInfo(controlName); this->add_control(controlName, convertBounds(info.getBounds()), convertBounds(info.getInitialBounds()), @@ -333,8 +328,8 @@ class MocoTropterSolver::TropterProblemBase : public tropter::Problem { // constraints. The base model never gets realized past // Stage::Velocity, so we don't ever need to set its controls. auto& osimControls = - m_mocoProbRep.getDiscreteControllerDisabledConstraints() - .updDiscreteControls(simTKStateDisabledConstraints); + m_mocoProbRep.getControlAllocatorDisabledConstraints() + .updControls(simTKStateDisabledConstraints); for (int ic = 0; ic < controls.size(); ++ic) { osimControls[ic] = controls[ic]; } @@ -369,9 +364,9 @@ class MocoTropterSolver::TropterProblemBase : public tropter::Problem { // point, so that each can preserve their cache? this->setSimTKState(in); - const auto& discreteController = - m_mocoProbRep.getDiscreteControllerDisabledConstraints(); - const auto& rawControls = discreteController.getDiscreteControls( + const auto& modelDisabledConstraints = + m_mocoProbRep.getModelDisabledConstraints(); + const auto& rawControls = modelDisabledConstraints.getControls( this->m_stateDisabledConstraints); // Compute the integrand for this cost term. @@ -394,11 +389,11 @@ class MocoTropterSolver::TropterProblemBase : public tropter::Problem { const auto& initialState = m_mocoProbRep.updStateDisabledConstraints(0); const auto& finalState = m_mocoProbRep.updStateDisabledConstraints(1); - const auto& discreteController = - m_mocoProbRep.getDiscreteControllerDisabledConstraints(); - const auto& initialRawControls = discreteController.getDiscreteControls( + const auto& modelDisabledConstraints = + m_mocoProbRep.getModelDisabledConstraints(); + const auto& initialRawControls = modelDisabledConstraints.getControls( initialState); - const auto& finalRawControls = discreteController.getDiscreteControls( + const auto& finalRawControls = modelDisabledConstraints.getControls( finalState); // Compute the cost for this cost term. From 8d74b5b41d5b6e77ae191672089f2f3fd69d7162 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 29 Jan 2024 11:35:38 -0800 Subject: [PATCH 40/48] Use raw controls from ControlAllocator in solvers --- .../MocoCasADiSolver/MocoCasADiSolver.cpp | 12 +-- .../Moco/MocoCasADiSolver/MocoCasOCProblem.h | 33 ++++--- OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.cpp | 6 +- OpenSim/Moco/MocoProblemRep.cpp | 53 +++++------ OpenSim/Moco/Test/testMocoActuators.cpp | 93 ++++++++++--------- OpenSim/Sandbox/Moco/sandboxSandbox.cpp | 49 +++------- 6 files changed, 114 insertions(+), 132 deletions(-) diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp index 82e9550190..7de2ced47e 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp @@ -377,12 +377,12 @@ MocoSolution MocoCasADiSolver::solveImpl() const { Logger::Level origLoggerLevel = Logger::getLevel(); Logger::setLevel(Logger::Level::Warn); CasOC::Solution casSolution; - try { - casSolution = casSolver->solve(casGuess); - } catch (...) { - OpenSim::Logger::setLevel(origLoggerLevel); - } - OpenSim::Logger::setLevel(origLoggerLevel); +// try { + casSolution = casSolver->solve(casGuess); +// } catch (...) { +// OpenSim::Logger::setLevel(origLoggerLevel); +// } +// OpenSim::Logger::setLevel(origLoggerLevel); MocoSolution mocoSolution = convertToMocoTrajectory(casSolution); diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h index 695da0b54c..8f064ec4bc 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h @@ -345,9 +345,9 @@ class MocoCasOCProblem : public CasOC::Problem { auto& simtkStateDisabledConstraints = mocoProblemRep->updStateDisabledConstraints(); - const auto& modelDisabledConstraints = - mocoProblemRep->getModelDisabledConstraints(); - const auto& rawControls = modelDisabledConstraints.getControls( + const auto& controlAllocatorDisabledConstraints = + mocoProblemRep->getControlAllocatorDisabledConstraints(); + const auto& rawControls = controlAllocatorDisabledConstraints.getControls( simtkStateDisabledConstraints); integrand = mocoCost.calcIntegrand( @@ -376,11 +376,13 @@ class MocoCasOCProblem : public CasOC::Problem { auto& simtkStateDisabledConstraintsFinal = mocoProblemRep->updStateDisabledConstraints(1); - const auto& modelDisabledConstraints = - mocoProblemRep->getModelDisabledConstraints(); - const auto& rawControlsInitial = modelDisabledConstraints.getControls( + const auto& controlAllocatorDisabledConstraints = + mocoProblemRep->getControlAllocatorDisabledConstraints(); + const auto& rawControlsInitial = + controlAllocatorDisabledConstraints.getControls( simtkStateDisabledConstraintsInitial); - const auto& rawControlsFinal = modelDisabledConstraints.getControls( + const auto& rawControlsFinal = + controlAllocatorDisabledConstraints.getControls( simtkStateDisabledConstraintsFinal); // Compute the cost for this cost term. @@ -410,9 +412,10 @@ class MocoCasOCProblem : public CasOC::Problem { auto& simtkStateDisabledConstraints = mocoProblemRep->updStateDisabledConstraints(); - const auto& modelDisabledConstraints = - mocoProblemRep->getModelDisabledConstraints(); - const auto& rawControls = modelDisabledConstraints.getControls( + const auto& controlAllocatorDisabledConstraints = + mocoProblemRep->getControlAllocatorDisabledConstraints(); + const auto& rawControls = + controlAllocatorDisabledConstraints.getControls( simtkStateDisabledConstraints); integrand = mocoEC.calcIntegrand( @@ -442,11 +445,13 @@ class MocoCasOCProblem : public CasOC::Problem { auto& simtkStateDisabledConstraintsFinal = mocoProblemRep->updStateDisabledConstraints(1); - const auto& modelDisabledConstraints = - mocoProblemRep->getModelDisabledConstraints(); - const auto& rawControlsInitial = modelDisabledConstraints.getControls( + const auto& controlAllocatorDisabledConstraints = + mocoProblemRep->getControlAllocatorDisabledConstraints(); + const auto& rawControlsInitial = + controlAllocatorDisabledConstraints.getControls( simtkStateDisabledConstraintsInitial); - const auto& rawControlsFinal = modelDisabledConstraints.getControls( + const auto& rawControlsFinal = + controlAllocatorDisabledConstraints.getControls( simtkStateDisabledConstraintsFinal); // Compute the cost for this cost term. diff --git a/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.cpp b/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.cpp index f815899f18..118b91c1fc 100644 --- a/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.cpp @@ -122,7 +122,7 @@ void MocoPeriodicityGoal::initializeOnModelImpl(const Model& model) const { setRequirements( 0, (int)m_indices_states.size() + (int)m_indices_controls.size(), - SimTK::Stage::Time); + SimTK::Stage::Velocity); } void MocoPeriodicityGoal::calcGoalImpl( @@ -139,8 +139,8 @@ void MocoPeriodicityGoal::calcGoalImpl( ++i; } - const auto& initialControls = input.initial_controls; - const auto& finalControls = input.final_controls; + const auto& initialControls = getModel().getControls(input.initial_state); + const auto& finalControls = getModel().getControls(input.final_state); int j = 0; for (const auto& index_control : m_indices_controls) { goal[i + j] = (initialControls[std::get<0>(index_control)] * diff --git a/OpenSim/Moco/MocoProblemRep.cpp b/OpenSim/Moco/MocoProblemRep.cpp index 65f633046e..c38cf2a0a0 100644 --- a/OpenSim/Moco/MocoProblemRep.cpp +++ b/OpenSim/Moco/MocoProblemRep.cpp @@ -120,7 +120,6 @@ void MocoProblemRep::initialize() { // Add the non-controlled, enabled actuators to an ActuatorInputController. auto actuatorController = make_unique(); - actuatorController->setName("actuator_controller"); for (const auto& actu : m_model_base.getComponentList()) { bool isControlled = std::find(controlledActuatorPaths.begin(), controlledActuatorPaths.end(), @@ -132,6 +131,14 @@ void MocoProblemRep::initialize() { } m_model_base.addController(actuatorController.release()); + // Finalize Actuator socket connections for all InputControllers. This is + // so we can get the names of the control names from the controlled + // actuator while skipping the Input connections. + for (auto& controller : m_model_base.updComponentList()) { + controller.updSocket("actuators").finalizeConnection( + m_model_base); + } + // Add a ControlAllocator to the model. This component will distribute the // OCP controls to all InputControllers in the model, including the // ActuatorInputController we just added to control actuators that are not @@ -148,16 +155,11 @@ void MocoProblemRep::initialize() { // Wire the ControlAllocators to the InputControllers in the model. for (auto& controller : m_model_base.updComponentList()) { - const auto& socket = controller.getSocket("actuators"); + auto& socket = controller.updSocket("actuators"); for (int i = 0; i < socket.getNumConnectees(); ++i) { const auto& actu = socket.getConnectee(i); const auto& actuPath = actu.getAbsolutePathString(); if (actu.numControls() > 1) { - const auto& channel = - m_control_allocator_base->getOutput("controls") - .getChannel(actuPath); - controller.connectInput_controls(channel, actuPath); - } else { for (int ic = 0; ic < actu.numControls(); ++ic) { std::string controlName = actuPath + "_" + std::to_string(ic); @@ -166,9 +168,16 @@ void MocoProblemRep::initialize() { .getChannel(controlName); controller.connectInput_controls(channel, controlName); } + } else { + const auto& channel = + m_control_allocator_base->getOutput("controls") + .getChannel(actuPath); + controller.connectInput_controls(channel, actuPath); } } } + m_model_base.finalizeConnections(); + m_model_base.initSystem(); // Scale factors // ------------- @@ -249,6 +258,7 @@ void MocoProblemRep::initialize() { // If there's a PrescribedMotion in the model, it's disabled by default // in this copied model. m_model_disabled_constraints = Model(m_model_base); + m_model_disabled_constraints.finalizeConnections(); // The constraint forces will be applied to the copied model via an // OpenSim::DiscreteForces component, a thin wrapper to Simbody's @@ -500,18 +510,12 @@ void MocoProblemRep::initialize() { // Control infos. // -------------- - // TODO wait, this looks wrong... - auto controlNames = createControlNamesFromModel(m_model_base); - auto actuatorInputControlNames = + // Check for control infos for the actuators in the ActuatorInputController + // since these are the only actuators that will have associated optimal + // control variables. + auto controlNames = createControlNamesForControllerType( m_model_base); - // Remove the actuators controlled by an ActuatorInputController from - // controlNames. TODO wait, shouldn't these be the only ones to check...? - for (const auto& name : actuatorInputControlNames) { - controlNames.erase(std::remove(controlNames.begin(), - controlNames.end(), name), controlNames.end()); - } - for (int i = 0; i < ph0.getProperty_control_infos_pattern().size(); ++i) { const auto& pattern = ph0.get_control_infos_pattern(i).getName(); auto regexPattern = std::regex(pattern); @@ -537,18 +541,11 @@ void MocoProblemRep::initialize() { m_control_infos[name] = ph0.get_control_infos(i); } - // Loop through the actuators in the ActuatorInputController since these are - // the only actuators that will have associated optimal control variables. - // Create control infos for actuators that do not have a control info. - // TODO this will change if we allow OCP controls on top of controls from - // controllers. + // Loop through the actuators in the ActuatorInputController again and + // create control infos for actuators that do not have a control info. const auto& actuController = - m_model_base.getComponent( - "actuator_controller"); - const auto& socket = actuController.getSocket("actuators"); - - // TODO use createControlNamesForControllerType() here? Then - // do we disallow user added ActuatorInputControllers? + m_model_base.getComponentList().begin(); + const auto& socket = actuController->getSocket("actuators"); for (int i = 0; i < socket.getNumConnectees(); ++i) { const auto& actu = socket.getConnectee(i); const std::string actuName = actu.getAbsolutePathString(); diff --git a/OpenSim/Moco/Test/testMocoActuators.cpp b/OpenSim/Moco/Test/testMocoActuators.cpp index 3df62dd180..190533574b 100644 --- a/OpenSim/Moco/Test/testMocoActuators.cpp +++ b/OpenSim/Moco/Test/testMocoActuators.cpp @@ -28,53 +28,54 @@ #include "Testing.h" using namespace OpenSim; - -Model createHangingMuscleModel(double optimalFiberLength, - double tendonSlackLength, bool ignoreActivationDynamics, - bool ignoreTendonCompliance, bool isTendonDynamicsExplicit) { - Model model; - model.setName("isometric_muscle"); - model.set_gravity(SimTK::Vec3(9.81, 0, 0)); - auto* body = new Body("body", 0.5, SimTK::Vec3(0), SimTK::Inertia(0)); - model.addComponent(body); - - // Allows translation along x. - auto* joint = new SliderJoint("joint", model.getGround(), *body); - auto& coord = joint->updCoordinate(SliderJoint::Coord::TranslationX); - coord.setName("height"); - model.addComponent(joint); - - auto* actu = new DeGrooteFregly2016Muscle(); - actu->setName("muscle"); - actu->set_max_isometric_force(10.0); - actu->set_optimal_fiber_length(optimalFiberLength); - actu->set_tendon_slack_length(tendonSlackLength); - actu->set_tendon_strain_at_one_norm_force(0.10); - actu->set_ignore_activation_dynamics(ignoreActivationDynamics); - actu->set_ignore_tendon_compliance(ignoreTendonCompliance); - actu->set_fiber_damping(0.01); - if (!isTendonDynamicsExplicit) { - actu->set_tendon_compliance_dynamics_mode("implicit"); +namespace { + Model createHangingMuscleModel(double optimalFiberLength, + double tendonSlackLength, bool ignoreActivationDynamics, + bool ignoreTendonCompliance, bool isTendonDynamicsExplicit) { + Model model; + model.setName("isometric_muscle"); + model.set_gravity(SimTK::Vec3(9.81, 0, 0)); + auto* body = new Body("body", 0.5, SimTK::Vec3(0), SimTK::Inertia(0)); + model.addComponent(body); + + // Allows translation along x. + auto* joint = new SliderJoint("joint", model.getGround(), *body); + auto& coord = joint->updCoordinate(SliderJoint::Coord::TranslationX); + coord.setName("height"); + model.addComponent(joint); + + auto* actu = new DeGrooteFregly2016Muscle(); + actu->setName("muscle"); + actu->set_max_isometric_force(10.0); + actu->set_optimal_fiber_length(optimalFiberLength); + actu->set_tendon_slack_length(tendonSlackLength); + actu->set_tendon_strain_at_one_norm_force(0.10); + actu->set_ignore_activation_dynamics(ignoreActivationDynamics); + actu->set_ignore_tendon_compliance(ignoreTendonCompliance); + actu->set_fiber_damping(0.01); + if (!isTendonDynamicsExplicit) { + actu->set_tendon_compliance_dynamics_mode("implicit"); + } + actu->set_max_contraction_velocity(10); + actu->set_pennation_angle_at_optimal(0); + actu->addNewPathPoint("origin", model.updGround(), SimTK::Vec3(0)); + actu->addNewPathPoint("insertion", *body, SimTK::Vec3(0)); + model.addForce(actu); + + body->attachGeometry(new Sphere(0.05)); + + CMC_TaskSet tasks; + CMC_Joint task; + task.setName(coord.getName()); + task.setCoordinateName(coord.getName()); + task.setKP(100, 1, 1); + task.setKV(20, 1, 1); + task.setActive(true, false, false); + tasks.cloneAndAppend(task); + tasks.print("hanging_muscle_cmc_tasks.xml"); + + return model; } - actu->set_max_contraction_velocity(10); - actu->set_pennation_angle_at_optimal(0); - actu->addNewPathPoint("origin", model.updGround(), SimTK::Vec3(0)); - actu->addNewPathPoint("insertion", *body, SimTK::Vec3(0)); - model.addForce(actu); - - body->attachGeometry(new Sphere(0.05)); - - CMC_TaskSet tasks; - CMC_Joint task; - task.setName(coord.getName()); - task.setCoordinateName(coord.getName()); - task.setKP(100, 1, 1); - task.setKV(20, 1, 1); - task.setActive(true, false, false); - tasks.cloneAndAppend(task); - tasks.print("hanging_muscle_cmc_tasks.xml"); - - return model; } TEMPLATE_TEST_CASE( diff --git a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp index 0ddd4eecb4..cebe7ef04d 100644 --- a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp +++ b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp @@ -19,51 +19,30 @@ // This file provides a way to easily prototype or test temporary snippets of // code during development. +#include "OpenSim/Actuators/ActivationCoordinateActuator.h" #include "OpenSim/Actuators/CoordinateActuator.h" #include "OpenSim/Actuators/ModelFactory.h" -#include -#include +#include #include +#include using namespace OpenSim; int main() { + auto model = ModelFactory::createSlidingPointMass(); + auto* actu = new ActivationCoordinateActuator(); + actu->setName("aca"); + actu->setCoordinate(&model.updCoordinateSet().get("position")); + actu->setMinControl(-0.31); + actu->setMaxControl(0.78); + model.addForce(actu); + MocoStudy study; + auto& problem = study.updProblem(); + problem.setModelAsCopy(model); + auto rep = problem.createRep(); - Model model = ModelFactory::createDoublePendulum(); - model.initSystem(); - - ControlAllocator* allocator = new ControlAllocator; - allocator->setName("control_allocator"); - allocator->addControl("/tau0"); - allocator->addControl("/tau1"); - model.addComponent(allocator); - - ActuatorInputController* controller = new ActuatorInputController; - controller->setName("actuator_controller"); - controller->addActuator(model.getComponent("/tau0")); - controller->addActuator(model.getComponent("/tau1")); - controller->connectInput_controls( - allocator->getOutput("controls").getChannel("/tau0"), "/tau0"); - controller->connectInput_controls( - allocator->getOutput("controls").getChannel("/tau1"), "/tau1"); - model.addController(controller); - model.finalizeFromProperties(); - model.finalizeConnections(); - - SimTK::State state = model.initSystem(); - - SimTK::Vector newControls(2, 0.0); - newControls[0] = 1.23; - newControls[1] = 4.56; - allocator->setControls(state, newControls); - model.realizeDynamics(state); - - model.printSubcomponentInfo(); - - std::cout << "actuation tau0: " << model.getComponent("/tau0").getActuation(state) << std::endl; - std::cout << "actuation tau1: " << model.getComponent("/tau1").getActuation(state) << std::endl; return EXIT_SUCCESS; } From 7a6d42296d6a1f55d131a68422fcf2d0cc910775 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 29 Jan 2024 15:16:51 -0800 Subject: [PATCH 41/48] Remove DiscreteController --- .../MocoCasADiSolver/MocoCasADiSolver.cpp | 12 +-- .../MocoCasADiSolver/MocoCasOCProblem.cpp | 2 - .../Moco/MocoCasADiSolver/MocoCasOCProblem.h | 4 +- OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.cpp | 3 + OpenSim/Moco/MocoProblemRep.h | 16 +--- OpenSim/Moco/Test/testMocoInterface.cpp | 2 +- OpenSim/Moco/tropter/TropterProblem.h | 19 ++-- .../Simulation/Control/DiscreteController.cpp | 86 ------------------- .../Simulation/Control/DiscreteController.h | 50 ----------- 9 files changed, 25 insertions(+), 169 deletions(-) delete mode 100644 OpenSim/Simulation/Control/DiscreteController.cpp delete mode 100644 OpenSim/Simulation/Control/DiscreteController.h diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp index 7de2ced47e..82e9550190 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasADiSolver.cpp @@ -377,12 +377,12 @@ MocoSolution MocoCasADiSolver::solveImpl() const { Logger::Level origLoggerLevel = Logger::getLevel(); Logger::setLevel(Logger::Level::Warn); CasOC::Solution casSolution; -// try { - casSolution = casSolver->solve(casGuess); -// } catch (...) { -// OpenSim::Logger::setLevel(origLoggerLevel); -// } -// OpenSim::Logger::setLevel(origLoggerLevel); + try { + casSolution = casSolver->solve(casGuess); + } catch (...) { + OpenSim::Logger::setLevel(origLoggerLevel); + } + OpenSim::Logger::setLevel(origLoggerLevel); MocoSolution mocoSolution = convertToMocoTrajectory(casSolution); diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp index e6d2795340..1a4b70c5db 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp @@ -66,8 +66,6 @@ MocoCasOCProblem::MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver, auto controlNames = problemRep.getControlAllocatorBase().getControlNamesInOrder(); for (const auto& controlName : controlNames) { - // TODO will need to check for "input" infos once we support user - // InputControllers. const auto& info = problemRep.getControlInfo(controlName); addControl(controlName, convertBounds(info.getBounds()), convertBounds(info.getInitialBounds()), diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h index 8f064ec4bc..7e0c73096e 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h @@ -347,7 +347,8 @@ class MocoCasOCProblem : public CasOC::Problem { const auto& controlAllocatorDisabledConstraints = mocoProblemRep->getControlAllocatorDisabledConstraints(); - const auto& rawControls = controlAllocatorDisabledConstraints.getControls( + const auto& rawControls = + controlAllocatorDisabledConstraints.getControls( simtkStateDisabledConstraints); integrand = mocoCost.calcIntegrand( @@ -755,7 +756,6 @@ class MocoCasOCProblem : public CasOC::Problem { bool m_paramsRequireInitSystem = true; std::string m_formattedTimeString; std::unordered_map m_yIndexMap; - //std::vector m_modelControlIndices; std::unique_ptr m_fileDeletionThrower; // Local memory to hold constraint forces. static thread_local SimTK::Vector_ diff --git a/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.cpp b/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.cpp index 118b91c1fc..993222a217 100644 --- a/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.cpp @@ -58,6 +58,7 @@ MocoPeriodicityGoal::MocoPeriodicityGoal() { constructProperties(); } void MocoPeriodicityGoal::constructProperties() { constructProperty_state_pairs(); constructProperty_control_pairs(); + constructProperty_ignore_controlled_actuators(false); } void MocoPeriodicityGoal::initializeOnModelImpl(const Model& model) const { @@ -139,7 +140,9 @@ void MocoPeriodicityGoal::calcGoalImpl( ++i; } + getModel().realizeVelocity(input.initial_state); const auto& initialControls = getModel().getControls(input.initial_state); + getModel().realizeVelocity(input.final_state); const auto& finalControls = getModel().getControls(input.final_state); int j = 0; for (const auto& index_control : m_indices_controls) { diff --git a/OpenSim/Moco/MocoProblemRep.h b/OpenSim/Moco/MocoProblemRep.h index fd1125ff93..e6ed033683 100644 --- a/OpenSim/Moco/MocoProblemRep.h +++ b/OpenSim/Moco/MocoProblemRep.h @@ -3,7 +3,7 @@ /* -------------------------------------------------------------------------- * * OpenSim: MocoProblemRep.h * * -------------------------------------------------------------------------- * - * Copyright (c) 2017 Stanford University and the Authors * + * Copyright (c) 2024 Stanford University and the Authors * * * * Author(s): Christopher Dembia, Nicholas Bianco * * * @@ -31,7 +31,6 @@ namespace OpenSim { class MocoProblem; -//class DiscreteController; class ControlAllocator; class DiscreteForces; class PositionMotion; @@ -56,8 +55,8 @@ class AccelerationMotion; /// If kinematics are not prescribed (with PositionMotion), /// ModelDisabledConstraints also contains an AccelerationMotion component, /// which is used by solvers that rely on implicit multibody dynamics. -/// The initialize() function adds a DiscreteController -/// to both models; this controller is used by a solver to set the control +/// The initialize() function adds a ControlAllocator component +/// to both models; this component is used by a solver to set the control /// signals for actuators to use. /// To learn the need for and use of these two models, see @ref impldiverse. @@ -90,9 +89,6 @@ class OSIMMOCO_API MocoProblemRep { SimTK::State& updStateBase() const { return m_state_base; } /// This is a component inside ModelBase that you can use to /// set the value of control signals. -// const DiscreteController& getDiscreteControllerBase() const { -// return m_discrete_controller_base.getRef(); -// } const ControlAllocator& getControlAllocatorBase() const { return m_control_allocator_base.getRef(); } @@ -121,9 +117,6 @@ class OSIMMOCO_API MocoProblemRep { } /// This is a component inside ModelDisabledConstraints that you can use to /// set the value of control signals. -// const DiscreteController& getDiscreteControllerDisabledConstraints() const { -// return m_discrete_controller_disabled_constraints.getRef(); -// } const ControlAllocator& getControlAllocatorDisabledConstraints() const { return m_control_allocator_disabled_constraints.getRef(); } @@ -386,14 +379,11 @@ class OSIMMOCO_API MocoProblemRep { Model m_model_base; mutable SimTK::State m_state_base; - //SimTK::ReferencePtr m_discrete_controller_base; SimTK::ReferencePtr m_control_allocator_base; SimTK::ReferencePtr m_position_motion_base; Model m_model_disabled_constraints; mutable std::array m_state_disabled_constraints; -// SimTK::ReferencePtr -// m_discrete_controller_disabled_constraints; SimTK::ReferencePtr m_control_allocator_disabled_constraints; SimTK::ReferencePtr diff --git a/OpenSim/Moco/Test/testMocoInterface.cpp b/OpenSim/Moco/Test/testMocoInterface.cpp index 72638e6f0f..1f05e0a58c 100644 --- a/OpenSim/Moco/Test/testMocoInterface.cpp +++ b/OpenSim/Moco/Test/testMocoInterface.cpp @@ -2163,7 +2163,7 @@ TEST_CASE("generateSpeedsFromValues() does not overwrite auxiliary states.") { } TEST_CASE("generateAccelerationsFromXXX() does not overwrite existing " - "non-acceleration derivatives.") { + "derivatives.") { int N = 20; SimTK::Vector time = createVectorLinspace(20, 0.0, 1.0); std::vector snames{"/jointset/joint/coord/value", diff --git a/OpenSim/Moco/tropter/TropterProblem.h b/OpenSim/Moco/tropter/TropterProblem.h index e9f19495bc..714aa1af61 100644 --- a/OpenSim/Moco/tropter/TropterProblem.h +++ b/OpenSim/Moco/tropter/TropterProblem.h @@ -364,9 +364,10 @@ class MocoTropterSolver::TropterProblemBase : public tropter::Problem { // point, so that each can preserve their cache? this->setSimTKState(in); - const auto& modelDisabledConstraints = - m_mocoProbRep.getModelDisabledConstraints(); - const auto& rawControls = modelDisabledConstraints.getControls( + const auto& controlAllocatorDisabledConstraints = + m_mocoProbRep.getControlAllocatorDisabledConstraints(); + const auto& rawControls = + controlAllocatorDisabledConstraints.getControls( this->m_stateDisabledConstraints); // Compute the integrand for this cost term. @@ -389,12 +390,12 @@ class MocoTropterSolver::TropterProblemBase : public tropter::Problem { const auto& initialState = m_mocoProbRep.updStateDisabledConstraints(0); const auto& finalState = m_mocoProbRep.updStateDisabledConstraints(1); - const auto& modelDisabledConstraints = - m_mocoProbRep.getModelDisabledConstraints(); - const auto& initialRawControls = modelDisabledConstraints.getControls( - initialState); - const auto& finalRawControls = modelDisabledConstraints.getControls( - finalState); + const auto& controlAllocatorDisabledConstraints = + m_mocoProbRep.getControlAllocatorDisabledConstraints(); + const auto& initialRawControls = + controlAllocatorDisabledConstraints.getControls(initialState); + const auto& finalRawControls = + controlAllocatorDisabledConstraints.getControls(finalState); // Compute the cost for this cost term. const auto& cost = m_mocoProbRep.getCostByIndex(cost_index); diff --git a/OpenSim/Simulation/Control/DiscreteController.cpp b/OpenSim/Simulation/Control/DiscreteController.cpp deleted file mode 100644 index b5b35cad41..0000000000 --- a/OpenSim/Simulation/Control/DiscreteController.cpp +++ /dev/null @@ -1,86 +0,0 @@ -/* -------------------------------------------------------------------------- * - * OpenSim Moco: DiscreteController.cpp * - * -------------------------------------------------------------------------- * - * Copyright (c) 2020 Stanford University and the Authors * - * * - * Author(s): Christopher 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 "DiscreteController.h" - -#include -#include -#include - -using namespace OpenSim; - -void DiscreteController::setDiscreteControls(SimTK::State& s, - const SimTK::Vector& controls) const { - updDiscreteControls(s) = controls; -} - -SimTK::Vector& DiscreteController::updDiscreteControls(SimTK::State& s) const { - const SimTK::Subsystem& subSys = getSystem().getDefaultSubsystem(); - auto& dv = subSys.updDiscreteVariable(s, m_discreteVarIndex); - auto& discreteControls = SimTK::Value::updDowncast(dv).upd(); - return discreteControls; -} - -const SimTK::Vector& DiscreteController::getDiscreteControls( - const SimTK::State& s) const { - const SimTK::Subsystem& subSys = getSystem().getDefaultSubsystem(); - auto& dv = subSys.getDiscreteVariable(s, m_discreteVarIndex); - auto& discreteControls = SimTK::Value::downcast(dv).get(); - return discreteControls; -} - -void DiscreteController::computeControls( - const SimTK::State& s, SimTK::Vector& controls) const { - const SimTK::Subsystem& subSys = getSystem().getDefaultSubsystem(); - const auto& dv = subSys.getDiscreteVariable(s, m_discreteVarIndex) ; - const auto& discreteControls = - SimTK::Value::downcast(dv).get(); - for (int i = 0; i < (int)m_controlIndices.size(); ++i) { - controls[m_controlIndices[i]] += discreteControls[i]; - } -} - -void DiscreteController::extendRealizeTopology(SimTK::State& state) const { - Super::extendRealizeTopology(state); - const SimTK::Subsystem& subSys = getSystem().getDefaultSubsystem(); - - // Get the control indexes for the actuators that are in the actuator set. - // This logic stores the indexes in the order of actuators stored in the - // model, skipping over indexes for any actuators not included in the - // DiscreteController's actuator set. - int count = 0; - for (const auto& actu : getModel().getComponentList()) { - const int nc = actu.numControls(); - for (int ic = 0; ic < nc; ++ic) { - const auto& socket = getSocket("actuators"); - for (int i = 0; i < socket.getNumConnectees(); ++i) { - if (socket.getConnecteePath(i) == actu.getAbsolutePath()) { - m_controlIndices.push_back(count);; - break; - } - } - count++; - } - } - - const SimTK::Vector initControls( - static_cast(m_controlIndices.size()), 0.0); - m_discreteVarIndex = - subSys.allocateDiscreteVariable(state, SimTK::Stage::Dynamics, - new SimTK::Value(initControls)); -} diff --git a/OpenSim/Simulation/Control/DiscreteController.h b/OpenSim/Simulation/Control/DiscreteController.h deleted file mode 100644 index ad2b34a693..0000000000 --- a/OpenSim/Simulation/Control/DiscreteController.h +++ /dev/null @@ -1,50 +0,0 @@ -#ifndef OPENSIM_DISCRETECONTROLLER_H -#define OPENSIM_DISCRETECONTROLLER_H -/* -------------------------------------------------------------------------- * - * OpenSim: DiscreteController.h * - * -------------------------------------------------------------------------- * - * Copyright (c) 2020 Stanford University and the Authors * - * * - * Author(s): Christopher 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 "Controller.h" - -namespace OpenSim { - -/** This component is used internally by Moco for passing a solver's control -variables to a Model. Use `Controller::addActuator()` to add the actuators -that will be controlled by this controller. Actuators can be added in any order, -but the control indexes for these actuators will be stored in the order used by -the model. -*/ -class OSIMSIMULATION_API DiscreteController : public Controller { - OpenSim_DECLARE_CONCRETE_OBJECT(DiscreteController, Controller); -public: - DiscreteController() = default; - void setDiscreteControls(SimTK::State& s, - const SimTK::Vector& controls) const; - SimTK::Vector& updDiscreteControls(SimTK::State& s) const; - const SimTK::Vector& getDiscreteControls(const SimTK::State& s) const; - void computeControls( - const SimTK::State& s, SimTK::Vector& controls) const override; -protected: - void extendRealizeTopology(SimTK::State&) const override; - mutable SimTK::DiscreteVariableIndex m_discreteVarIndex; - mutable std::vector m_controlIndices; - -}; - -} // namespace OpenSim - -#endif // OPENSIM_DISCRETECONTROLLER_H From 9d4c7c4c2ec953385b47dff94e1ebe99cdf0d54c Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 29 Jan 2024 15:51:48 -0800 Subject: [PATCH 42/48] Revert sandbox changes --- OpenSim/CMakeLists.txt | 2 +- OpenSim/Sandbox/Moco/sandboxSandbox.cpp | 19 ------------------- 2 files changed, 1 insertion(+), 20 deletions(-) diff --git a/OpenSim/CMakeLists.txt b/OpenSim/CMakeLists.txt index 8ba2fcc16e..8d53dedd8f 100644 --- a/OpenSim/CMakeLists.txt +++ b/OpenSim/CMakeLists.txt @@ -10,6 +10,6 @@ add_subdirectory(Moco) add_subdirectory(Examples) add_subdirectory(Tests) -add_subdirectory(Sandbox) +#add_subdirectory(Sandbox) install(FILES OpenSim.h DESTINATION "${CMAKE_INSTALL_INCLUDEDIR}/OpenSim") diff --git a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp index cebe7ef04d..8e30852df9 100644 --- a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp +++ b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp @@ -19,30 +19,11 @@ // This file provides a way to easily prototype or test temporary snippets of // code during development. -#include "OpenSim/Actuators/ActivationCoordinateActuator.h" -#include "OpenSim/Actuators/CoordinateActuator.h" -#include "OpenSim/Actuators/ModelFactory.h" - -#include #include -#include using namespace OpenSim; - int main() { - auto model = ModelFactory::createSlidingPointMass(); - auto* actu = new ActivationCoordinateActuator(); - actu->setName("aca"); - actu->setCoordinate(&model.updCoordinateSet().get("position")); - actu->setMinControl(-0.31); - actu->setMaxControl(0.78); - model.addForce(actu); - MocoStudy study; - auto& problem = study.updProblem(); - problem.setModelAsCopy(model); - auto rep = problem.createRep(); - return EXIT_SUCCESS; } From a96551db9f564c64391c4b9136c0be699b191e42 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Wed, 31 Jan 2024 12:26:48 -0800 Subject: [PATCH 43/48] Correct docs for ModelFactory sliding mass --- OpenSim/Actuators/ModelFactory.cpp | 3 ++- OpenSim/Moco/Test/testMocoInterface.cpp | 28 +++++++++++++++---------- 2 files changed, 19 insertions(+), 12 deletions(-) diff --git a/OpenSim/Actuators/ModelFactory.cpp b/OpenSim/Actuators/ModelFactory.cpp index 021fb0fa6d..8295841e53 100644 --- a/OpenSim/Actuators/ModelFactory.cpp +++ b/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. @@ -149,7 +150,7 @@ Model ModelFactory::createSlidingPointMass() { actu->setOptimalForce(1); actu->setMinControl(-10); actu->setMaxControl(10); - model.addForce(actu); + model.addComponent(actu); model.finalizeConnections(); return model; diff --git a/OpenSim/Moco/Test/testMocoInterface.cpp b/OpenSim/Moco/Test/testMocoInterface.cpp index 1f05e0a58c..cd39fbf491 100644 --- a/OpenSim/Moco/Test/testMocoInterface.cpp +++ b/OpenSim/Moco/Test/testMocoInterface.cpp @@ -50,6 +50,7 @@ namespace { model->setName("sliding_mass"); model->set_gravity(SimTK::Vec3(0, 0, 0)); auto* body = new Body("body", mass, SimTK::Vec3(0), SimTK::Inertia(0)); + body->attachGeometry(new Sphere(0.05)); model->addComponent(body); // Allows translation along x. @@ -2242,9 +2243,9 @@ TEMPLATE_TEST_CASE("Sliding mass with PrescribedController", "", { MocoStudy study; auto& problem = study.updProblem(); - auto model = createSlidingMassModel(); - problem.setModel(std::move(model)); - problem.setTimeBounds(0, 5); + Model model = ModelFactory::createSlidingPointMass(); + problem.setModelAsCopy(model); + problem.setTimeBounds(0, 2); problem.setStateInfo("/slider/position/value", {0, 1}, 0, 1); problem.setStateInfo("/slider/position/speed", {-100, 100}, 0, 0); problem.setControlInfo("/actuator", {-50, 50}); @@ -2252,6 +2253,7 @@ TEMPLATE_TEST_CASE("Sliding mass with PrescribedController", "", auto& solver = study.initSolver(); solver.set_num_mesh_intervals(50); MocoSolution solution = study.solve(); + solution.write("testMocoInterface_testSlidingMass_solution.sto"); statesTrajectory = solution.getStatesTrajectory(); controlsTable = solution.exportToControlsTable(); } @@ -2261,25 +2263,29 @@ TEMPLATE_TEST_CASE("Sliding mass with PrescribedController", "", // back. { const auto& time = controlsTable.getIndependentColumn(); - const auto& control = controlsTable.getDependentColumn("/actuator"); - auto model = createSlidingMassModel(); + const auto& control = + controlsTable.getDependentColumn("/actuator"); + Model model = ModelFactory::createSlidingPointMass(); auto* controller = new PrescribedController(); - controller->addActuator(model->getComponent("/actuator")); - controller->prescribeControlForActuator("actuator", + controller->addActuator(model.getComponent("/actuator")); + controller->prescribeControlForActuator("/actuator", new GCVSpline(5, control.size(), time.data(), &control[0], "/actuator", 0.0)); - model->addController(controller); - model->finalizeConnections(); + model.addController(controller); + model.finalizeConnections(); MocoStudy study; auto& problem = study.updProblem(); - problem.setModel(std::move(model)); - problem.setTimeBounds(0, 5); + problem.setModelAsCopy(model); + problem.setTimeBounds(0, 2); problem.setStateInfo("/slider/position/value", {0, 1}, 0); problem.setStateInfo("/slider/position/speed", {-100, 100}, 0); auto& solver = study.initSolver(); solver.set_num_mesh_intervals(50); MocoSolution solution = study.solve(); + study.visualize(solution); + solution.write( + "testMocoInterface_testSlidingMass_prescribed_solution.sto"); OpenSim_REQUIRE_MATRIX_ABSTOL(solution.getStatesTrajectory(), statesTrajectory, 1e-9); From aaa4b71980a1d8bb177fb497171bdeac4ebf3610 Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Mon, 5 Feb 2024 17:11:50 -0800 Subject: [PATCH 44/48] Respond to review comments --- Applications/Analyze/test/testAnalyzeTool.cpp | 1 - Applications/Forward/test/testForward.cpp | 6 +----- OpenSim/Simulation/Control/ControlSetController.cpp | 6 +++--- OpenSim/Simulation/Control/PrescribedController.h | 2 +- OpenSim/Simulation/Model/AbstractTool.cpp | 4 ++++ OpenSim/Tests/Wrapping/testWrapping.cpp | 2 +- OpenSim/Tools/CMCTool.cpp | 4 ++-- OpenSim/Tools/CMCTool.h | 2 +- OpenSim/Tools/Test/testControllers.cpp | 2 +- 9 files changed, 14 insertions(+), 15 deletions(-) diff --git a/Applications/Analyze/test/testAnalyzeTool.cpp b/Applications/Analyze/test/testAnalyzeTool.cpp index 79ded35a25..a9e46690a7 100644 --- a/Applications/Analyze/test/testAnalyzeTool.cpp +++ b/Applications/Analyze/test/testAnalyzeTool.cpp @@ -489,7 +489,6 @@ void testIMUDataReporter() { pendulum.getComponent("/tau0")); controller->addActuator( pendulum.getComponent("/tau1")); - controller->prescribeControlForActuator("tau0", new Constant(0.0)); // Specify constant torque functions to the torque actuators Constant* constantTorque0 = new Constant(10.0); Constant* constantTorque1 = new Constant(10.0); diff --git a/Applications/Forward/test/testForward.cpp b/Applications/Forward/test/testForward.cpp index d1d4b71e8f..78f24d6d0a 100644 --- a/Applications/Forward/test/testForward.cpp +++ b/Applications/Forward/test/testForward.cpp @@ -241,12 +241,8 @@ void testGait2354WithControllerGUI() { forward.setResultsDir(resultsDir); forward.updateModelForces(*model, ""); - - // This initSystem() call needs to happen after updateModelForces() and - // before setModel(). Calling initSystem() after setModel() will cause - // Controller connectee paths to be incorrect. - model->initSystem(); forward.setModel(*model); + forward.run(); // For good measure we'll make sure we still get the identical results diff --git a/OpenSim/Simulation/Control/ControlSetController.cpp b/OpenSim/Simulation/Control/ControlSetController.cpp index 732b45f558..ee67fc5afd 100644 --- a/OpenSim/Simulation/Control/ControlSetController.cpp +++ b/OpenSim/Simulation/Control/ControlSetController.cpp @@ -269,7 +269,6 @@ void ControlSetController::extendConnectToModel(Model& model) { std::string ext = ".excitation"; for (int i = 0; _controlSet != nullptr && i < _controlSet->getSize(); ++i) { std::string actName = _controlSet->get(i).getName(); - std::cout << "actName = " << actName << std::endl; if (actName.length() > ext.length() && !actName.compare( actName.length() - ext.length(), ext.length(), ext)) { @@ -281,7 +280,8 @@ void ControlSetController::extendConnectToModel(Model& model) { for (int iactu = 0; iactu < (int)socket.getNumConnectees(); ++iactu) { if (socket.getConnectee(iactu).getName() == actName) { log_cout("ControlSetController::extendConnectToModel " - "Actuator '{}' already connected to ControlSetController '{}'.", + "Actuator '{}' already connected to " + "ControlSetController '{}'.", actName, getName()); isConnected = true; break; @@ -293,7 +293,7 @@ void ControlSetController::extendConnectToModel(Model& model) { for (const auto& actu : model.getComponentList()) { if (actu.getName() == actName) { log_cout("ControlSetController::extendConnectToModel " - "Connecting ControlSetController '{}' to Actuator" + "Connecting ControlSetController '{}' to Actuator " "'{}'.", getName(), actu.getName()); addActuator(actu); isConnected = true; diff --git a/OpenSim/Simulation/Control/PrescribedController.h b/OpenSim/Simulation/Control/PrescribedController.h index c7af93570f..be7f480c3e 100644 --- a/OpenSim/Simulation/Control/PrescribedController.h +++ b/OpenSim/Simulation/Control/PrescribedController.h @@ -9,7 +9,7 @@ * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * * through the Warrior Web program. * * * - * Copyright (c) 2005-2023 Stanford University and the Authors * + * Copyright (c) 2005-2024 Stanford University and the Authors * * Author(s): Ajay Seth * * * * Licensed under the Apache License, Version 2.0 (the "License"); you may * diff --git a/OpenSim/Simulation/Model/AbstractTool.cpp b/OpenSim/Simulation/Model/AbstractTool.cpp index dcac598069..b714a8d857 100644 --- a/OpenSim/Simulation/Model/AbstractTool.cpp +++ b/OpenSim/Simulation/Model/AbstractTool.cpp @@ -411,6 +411,10 @@ updateModelForces(Model& model, const string &aToolSetupFileName, ForceSet *rOri ForceSet *forceSet=new ForceSet(_forceSetFiles[i], true); model.updForceSet().append(*forceSet); } + + // Build up the new system with the new forces added from the force set + // files. + model.initSystem(); } //_____________________________________________________________________________ /** diff --git a/OpenSim/Tests/Wrapping/testWrapping.cpp b/OpenSim/Tests/Wrapping/testWrapping.cpp index 2dd0237d1f..2a4ec2ef71 100644 --- a/OpenSim/Tests/Wrapping/testWrapping.cpp +++ b/OpenSim/Tests/Wrapping/testWrapping.cpp @@ -85,7 +85,7 @@ namespace { const auto& actuatorsSet = model.getActuators(); actuController.setActuators(actuatorsSet); const auto& socket = actuController.getSocket("actuators"); - for (int i = 0; i < (int)socket.getNumConnectees(); i++) { + for (int i = 0; i < (int)socket.getNumConnectees(); ++i) { actuController.prescribeControlForActuator( actuatorsSet.get(i).getAbsolutePathString(), new Constant(activation)); diff --git a/OpenSim/Tools/CMCTool.cpp b/OpenSim/Tools/CMCTool.cpp index 7c438a6b02..c720309c11 100644 --- a/OpenSim/Tools/CMCTool.cpp +++ b/OpenSim/Tools/CMCTool.cpp @@ -415,7 +415,7 @@ bool CMCTool::run() // Check to see if user has explicitly listed Actuators to be // excluded from CMC's control. std::vector actuatorPaths = - getActuatorsForCMC(_excludedActuators); + getActuatorsPathsForCMC(_excludedActuators); for (const auto& actuatorPath : actuatorPaths) { if (_model->hasComponent(actuatorPath) && !actuatorPath.empty() && actuatorPath != ComponentPath::root()) { @@ -1103,7 +1103,7 @@ void CMCTool::setOriginalForceSet(const ForceSet &aForceSet) { /* Get the Set of model actuators for CMC that exclude user specified Actuators */ -std::vector CMCTool::getActuatorsForCMC( +std::vector CMCTool::getActuatorsPathsForCMC( const Array& actuatorsByNameOrGroup) { const Set& actuatorsForCMC = _model->getActuators(); diff --git a/OpenSim/Tools/CMCTool.h b/OpenSim/Tools/CMCTool.h index 5f5e523a8f..da0e5667a1 100644 --- a/OpenSim/Tools/CMCTool.h +++ b/OpenSim/Tools/CMCTool.h @@ -143,7 +143,7 @@ OpenSim_DECLARE_CONCRETE_OBJECT(CMCTool, AbstractTool); /** Get the vector of model actuator paths for CMC that exclude user specified Actuators */ - std::vector getActuatorsForCMC( + std::vector getActuatorsPathsForCMC( const Array &actuatorsByNameOrGroup); //-------------------------------------------------------------------------- diff --git a/OpenSim/Tools/Test/testControllers.cpp b/OpenSim/Tools/Test/testControllers.cpp index d570221ccd..959937c8c9 100644 --- a/OpenSim/Tools/Test/testControllers.cpp +++ b/OpenSim/Tools/Test/testControllers.cpp @@ -7,7 +7,7 @@ * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA * * through the Warrior Web program. * * * - * Copyright (c) 2005-2023 Stanford University and the Authors * + * Copyright (c) 2005-2024 Stanford University and the Authors * * Author(s): Ajay Seth * * * * Licensed under the Apache License, Version 2.0 (the "License"); you may * From a0d5717f49d2fca38bae8d79eab8de9782c9206f Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Tue, 6 Feb 2024 17:25:57 -0800 Subject: [PATCH 45/48] Update change log and deprecated function message --- CHANGELOG.md | 6 +++++- OpenSim/Simulation/Control/PrescribedController.h | 9 +++++++-- 2 files changed, 12 insertions(+), 3 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index edbc25da39..28a4f7a02e 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -24,7 +24,11 @@ v4.6 - 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` (#3683) +- `Controller` now manages the list of controlled actuators using a list `Socket` instead of a `Set` (#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. v4.5 ==== diff --git a/OpenSim/Simulation/Control/PrescribedController.h b/OpenSim/Simulation/Control/PrescribedController.h index be7f480c3e..f43c9766c8 100644 --- a/OpenSim/Simulation/Control/PrescribedController.h +++ b/OpenSim/Simulation/Control/PrescribedController.h @@ -105,8 +105,13 @@ OpenSim_DECLARE_CONCRETE_OBJECT(PrescribedController, Controller); void prescribeControlForActuator(const std::string& actuLabel, Function* prescribedFunction); - [[deprecated("this method no longer does anything")]] - void prescribeControlForActuator(int index, Function* prescribedFunction) {} + [[deprecated("Use prescribeControlForActuator(const std::string&, Function*) instead")]] + void prescribeControlForActuator(int index, Function* prescribedFunction) { + OPENSIM_THROW_FRMOBJ(Exception, + "PrescribedController::prescribeControlForActuator(int, Function*) " + "is deprecated. Use prescribeControlForActuator(const std::string&, " + "Function*) instead."); + } protected: // MODEL COMPONENT INTERFACE From 408a792240de05457a1ef58dbc48a449e6fa150b Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Wed, 7 Feb 2024 11:35:36 -0800 Subject: [PATCH 46/48] Add deprecated PrescribedController method to changelog --- CHANGELOG.md | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 28a4f7a02e..c4516818b7 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -28,7 +28,8 @@ v4.6 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. + 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 ==== From 4947078d46b4e3503a494eb1d52afb9421edbd7c Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Sat, 10 Feb 2024 21:13:30 -0800 Subject: [PATCH 47/48] static cast to int warnings // revert ModelFactory changes to fix tests --- OpenSim/Actuators/ModelFactory.cpp | 2 +- OpenSim/Actuators/ModelFactory.h | 7 ++++--- OpenSim/Moco/MocoProblemRep.cpp | 6 +++--- OpenSim/Moco/Test/testMocoInterface.cpp | 13 +++++++------ OpenSim/Simulation/Control/InputController.cpp | 9 +++++---- OpenSim/Simulation/SimulationUtilities.h | 5 +++-- OpenSim/Tests/Wrapping/testWrapping.cpp | 6 +++--- 7 files changed, 26 insertions(+), 22 deletions(-) diff --git a/OpenSim/Actuators/ModelFactory.cpp b/OpenSim/Actuators/ModelFactory.cpp index 8295841e53..9c62626bc0 100644 --- a/OpenSim/Actuators/ModelFactory.cpp +++ b/OpenSim/Actuators/ModelFactory.cpp @@ -150,7 +150,7 @@ Model ModelFactory::createSlidingPointMass() { actu->setOptimalForce(1); actu->setMinControl(-10); actu->setMaxControl(10); - model.addComponent(actu); + model.addForce(actu); model.finalizeConnections(); return model; diff --git a/OpenSim/Actuators/ModelFactory.h b/OpenSim/Actuators/ModelFactory.h index ba0f530c6f..4ce697b90e 100644 --- a/OpenSim/Actuators/ModelFactory.h +++ b/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: diff --git a/OpenSim/Moco/MocoProblemRep.cpp b/OpenSim/Moco/MocoProblemRep.cpp index c38cf2a0a0..a637d3c6e6 100644 --- a/OpenSim/Moco/MocoProblemRep.cpp +++ b/OpenSim/Moco/MocoProblemRep.cpp @@ -113,7 +113,7 @@ void MocoProblemRep::initialize() { controller.getConcreteClassName()); } const auto& socket = controller.getSocket("actuators"); - for (int i = 0; i < socket.getNumConnectees(); ++i) { + for (int i = 0; i < static_cast(socket.getNumConnectees()); ++i) { controlledActuatorPaths.push_back(socket.getConnecteePath(i)); } } @@ -156,7 +156,7 @@ void MocoProblemRep::initialize() { // Wire the ControlAllocators to the InputControllers in the model. for (auto& controller : m_model_base.updComponentList()) { auto& socket = controller.updSocket("actuators"); - for (int i = 0; i < socket.getNumConnectees(); ++i) { + for (int i = 0; i < static_cast(socket.getNumConnectees()); ++i) { const auto& actu = socket.getConnectee(i); const auto& actuPath = actu.getAbsolutePathString(); if (actu.numControls() > 1) { @@ -546,7 +546,7 @@ void MocoProblemRep::initialize() { const auto& actuController = m_model_base.getComponentList().begin(); const auto& socket = actuController->getSocket("actuators"); - for (int i = 0; i < socket.getNumConnectees(); ++i) { + for (int i = 0; i < static_cast(socket.getNumConnectees()); ++i) { const auto& actu = socket.getConnectee(i); const std::string actuName = actu.getAbsolutePathString(); if (actu.numControls() == 1) { diff --git a/OpenSim/Moco/Test/testMocoInterface.cpp b/OpenSim/Moco/Test/testMocoInterface.cpp index cd39fbf491..63e41dfea1 100644 --- a/OpenSim/Moco/Test/testMocoInterface.cpp +++ b/OpenSim/Moco/Test/testMocoInterface.cpp @@ -2248,7 +2248,7 @@ TEMPLATE_TEST_CASE("Sliding mass with PrescribedController", "", problem.setTimeBounds(0, 2); problem.setStateInfo("/slider/position/value", {0, 1}, 0, 1); problem.setStateInfo("/slider/position/speed", {-100, 100}, 0, 0); - problem.setControlInfo("/actuator", {-50, 50}); + problem.setControlInfo("/forceset/actuator", {-50, 50}); problem.addGoal(); auto& solver = study.initSolver(); solver.set_num_mesh_intervals(50); @@ -2264,13 +2264,14 @@ TEMPLATE_TEST_CASE("Sliding mass with PrescribedController", "", { const auto& time = controlsTable.getIndependentColumn(); const auto& control = - controlsTable.getDependentColumn("/actuator"); + controlsTable.getDependentColumn("/forceset/actuator"); Model model = ModelFactory::createSlidingPointMass(); auto* controller = new PrescribedController(); - controller->addActuator(model.getComponent("/actuator")); - controller->prescribeControlForActuator("/actuator", + controller->addActuator( + model.getComponent("/forceset/actuator")); + controller->prescribeControlForActuator("/forceset/actuator", new GCVSpline(5, control.size(), time.data(), &control[0], - "/actuator", 0.0)); + "/forceset/actuator", 0.0)); model.addController(controller); model.finalizeConnections(); @@ -2283,7 +2284,7 @@ TEMPLATE_TEST_CASE("Sliding mass with PrescribedController", "", auto& solver = study.initSolver(); solver.set_num_mesh_intervals(50); MocoSolution solution = study.solve(); - study.visualize(solution); + //study.visualize(solution); solution.write( "testMocoInterface_testSlidingMass_prescribed_solution.sto"); diff --git a/OpenSim/Simulation/Control/InputController.cpp b/OpenSim/Simulation/Control/InputController.cpp index 000bb82ce5..a05a70e3fd 100644 --- a/OpenSim/Simulation/Control/InputController.cpp +++ b/OpenSim/Simulation/Control/InputController.cpp @@ -63,7 +63,7 @@ std::vector InputController::getInputChannelAliases() const { std::vector aliases; const auto& input = getInput("controls"); aliases.reserve(input.getNumConnectees()); - for (int i = 0; i < input.getNumConnectees(); ++i) { + for (int i = 0; i < static_cast(input.getNumConnectees()); ++i) { aliases.push_back(input.getAlias(i)); } @@ -88,7 +88,7 @@ void InputController::extendConnectToModel(Model& model) { m_controlNames.clear(); m_controlIndexes.clear(); const auto& socket = getSocket("actuators"); - for (int i = 0; i < socket.getNumConnectees(); ++i) { + for (int i = 0; i < static_cast(socket.getNumConnectees()); ++i) { const auto& actu = socket.getConnectee(i); if (actu.numControls() > 1) { // Non-scalar actuator. @@ -145,7 +145,7 @@ ActuatorInputController& ActuatorInputController::operator=( void ActuatorInputController::computeControls(const SimTK::State& s, SimTK::Vector& controls) const { const auto& input = getInput("controls"); - for (int i = 0; i < input.getNumConnectees(); ++i) { + for (int i = 0; i < static_cast(input.getNumConnectees()); ++i) { controls[m_controlIndexesInConnecteeOrder[i]] = input.getValue(s, i); } } @@ -162,7 +162,8 @@ void ActuatorInputController::extendConnectToModel(Model& model) { static_cast(controlNames.size())) const auto& input = getInput("controls"); - OPENSIM_THROW_IF(input.getNumConnectees() != getNumControls(), Exception, + OPENSIM_THROW_IF( + static_cast(input.getNumConnectees()) != getNumControls(), Exception, "Expected the number of Input connectees ({}) to match the number " "of actuators controls ({}), but they do not.", input.getNumConnectees(), getNumControls()); diff --git a/OpenSim/Simulation/SimulationUtilities.h b/OpenSim/Simulation/SimulationUtilities.h index 59db7b03e5..e50f0f0930 100644 --- a/OpenSim/Simulation/SimulationUtilities.h +++ b/OpenSim/Simulation/SimulationUtilities.h @@ -190,12 +190,13 @@ std::vector createControlNamesForControllerType( } for (const auto& controller : model.getComponentList()) { - const auto& socket = controller.getSocket("actuators"); + const auto& socket = + controller.template getSocket("actuators"); // Loop through all actuators and create control names. For scalar // actuators, use the actuator name for the control name. For non-scalar // actuators, use the actuator name with a control index appended for // the control name. - for (int i = 0; i < socket.getNumConnectees(); ++i) { + for (int i = 0; i < static_cast(socket.getNumConnectees()); ++i) { const auto& actu = socket.getConnectee(i); if (!actu.get_appliesForce()) { continue; diff --git a/OpenSim/Tests/Wrapping/testWrapping.cpp b/OpenSim/Tests/Wrapping/testWrapping.cpp index 2a4ec2ef71..71a0104297 100644 --- a/OpenSim/Tests/Wrapping/testWrapping.cpp +++ b/OpenSim/Tests/Wrapping/testWrapping.cpp @@ -376,7 +376,7 @@ TEST_CASE("testFunctionBasedPath") { auto* actu = new PathActuator(); actu->set_path(fbPath); - actu->setName("actuator"); + actu->setName("path_actuator"); actu->setOptimalForce(1); model.addComponent(actu); model.finalizeConnections(); @@ -445,7 +445,7 @@ TEST_CASE("testFunctionBasedPath") { auto* actu = new PathActuator(); actu->set_path(fbPath); - actu->setName("actuator"); + actu->setName("path_actuator"); actu->setOptimalForce(1); model.addComponent(actu); model.finalizeConnections(); @@ -545,7 +545,7 @@ TEST_CASE("testFunctionBasedPath") { auto* actu = new PathActuator(); actu->set_path(fbPath); - actu->setName("actuator"); + actu->setName("path_actuator"); actu->setOptimalForce(1); model.addComponent(actu); model.finalizeConnections(); From 28a89eef1cd51efcffddef7f7b0e058c58dc5b6e Mon Sep 17 00:00:00 2001 From: Nicholas Bianco Date: Sun, 11 Feb 2024 14:30:16 -0800 Subject: [PATCH 48/48] Remove InputController noexcepts --- OpenSim/Simulation/Control/InputController.cpp | 9 ++++----- OpenSim/Simulation/Control/InputController.h | 10 +++++----- 2 files changed, 9 insertions(+), 10 deletions(-) diff --git a/OpenSim/Simulation/Control/InputController.cpp b/OpenSim/Simulation/Control/InputController.cpp index a05a70e3fd..e5abaa4ce3 100644 --- a/OpenSim/Simulation/Control/InputController.cpp +++ b/OpenSim/Simulation/Control/InputController.cpp @@ -43,10 +43,9 @@ InputController::InputController(const InputController& other) = default; InputController& InputController::operator=(const InputController& other) = default; -InputController::InputController(InputController&& other) noexcept = default; +InputController::InputController(InputController&& other) = default; -InputController& -InputController::operator=(InputController&& other) noexcept = default; +InputController& InputController::operator=(InputController&& other) = default; //============================================================================= // METHODS @@ -134,10 +133,10 @@ ActuatorInputController& ActuatorInputController::operator=( const ActuatorInputController& other) = default; ActuatorInputController::ActuatorInputController( - ActuatorInputController&& other) noexcept = default; + ActuatorInputController&& other) = default; ActuatorInputController& ActuatorInputController::operator=( - ActuatorInputController&& other) noexcept = default; + ActuatorInputController&& other) = default; //============================================================================= // CONTROLLER INTERFACE diff --git a/OpenSim/Simulation/Control/InputController.h b/OpenSim/Simulation/Control/InputController.h index bd518156c5..a02c71e081 100644 --- a/OpenSim/Simulation/Control/InputController.h +++ b/OpenSim/Simulation/Control/InputController.h @@ -68,15 +68,15 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(InputController, Controller); // METHODS //============================================================================= - // CONSTRUCTION/DESCTRUCTION + // CONSTRUCTION AND DESTRUCTION InputController(); ~InputController() override; InputController(const InputController& other); InputController& operator=(const InputController& other); - InputController(InputController&& other) noexcept; - InputController& operator=(InputController&& other) noexcept; + InputController(InputController&& other); + InputController& operator=(InputController&& other); // METHODS /** @@ -153,8 +153,8 @@ class OSIMSIMULATION_API ActuatorInputController : public InputController { ActuatorInputController(const ActuatorInputController& other); ActuatorInputController& operator=(const ActuatorInputController& other); - ActuatorInputController(ActuatorInputController&& other) noexcept; - ActuatorInputController& operator=(ActuatorInputController&& other) noexcept; + ActuatorInputController(ActuatorInputController&& other); + ActuatorInputController& operator=(ActuatorInputController&& other); // CONTROLLER INTERFACE void computeControls(