diff --git a/.gitignore b/.gitignore index 9ceeb60e51..33e1d65e89 100644 --- a/.gitignore +++ b/.gitignore @@ -55,6 +55,7 @@ delete_this_to_stop_optimization*.txt .idea/editor.xml .idea/codeStyles/codeStyleConfig.xml .idea/codeStyles/Project.xml +.idea/editor.xml # Sensitive or high-churn files: .idea/**/dataSources/ diff --git a/Applications/Analyze/test/testAnalyzeTool.cpp b/Applications/Analyze/test/testAnalyzeTool.cpp index 5f9629edf7..a9e46690a7 100644 --- a/Applications/Analyze/test/testAnalyzeTool.cpp +++ b/Applications/Analyze/test/testAnalyzeTool.cpp @@ -489,19 +489,12 @@ 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. + // 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/Applications/Forward/test/testForward.cpp b/Applications/Forward/test/testForward.cpp index 257e9a3f67..78f24d6d0a 100644 --- a/Applications/Forward/test/testForward.cpp +++ b/Applications/Forward/test/testForward.cpp @@ -35,7 +35,7 @@ using namespace std; void testPendulum(); void testPendulumExternalLoad(); -void testPendulumExternalLoadWithPointInGround(); +void testPendulumExternalLoadWithPointInGround(); void testArm26(); void testGait2354(); void testGait2354WithController(); @@ -95,7 +95,7 @@ void testPendulumExternalLoad() { ASSERT(results.getLastTime() == 1.0); Storage standard("Results/pendulum_states.sto"); - + Array data; int i = results.getSize() - 1; @@ -104,10 +104,10 @@ void testPendulumExternalLoad() { data.setSize(state->getSize()); standard.getDataAtTime(time, state->getSize(), data); int nc = forward.getModel().getNumCoordinates(); - for (int j = 0; j < nc; ++j) { + for (int j = 0; j < nc; ++j) { stringstream message; - message << "t=" << time <<" state# "<< j << " " - << standard.getColumnLabels()[j+1] << " std=" << data[j] + message << "t=" << time <<" state# "<< j << " " + << standard.getColumnLabels()[j+1] << " std=" << data[j] <<" computed=" << state->getData()[j]; ASSERT_EQUAL(data[j], state->getData()[j], 1e-2, __FILE__, __LINE__, "ASSERT_EQUAL FAILED " + message.str()); @@ -132,7 +132,7 @@ void testPendulumExternalLoadWithPointInGround() { data.setSize(state->getSize()); standard.getDataAtTime(time, state->getSize(), data); int nc = forward.getModel().getNumCoordinates(); - for (int j = 0; j < nc; ++j) { + for (int j = 0; j < nc; ++j) { stringstream message; message << "t=" << time <<" state# "<< j << " " << standard.getColumnLabels()[j+1] << " std=" << data[j] <<" computed=" << state->getData()[j]; @@ -160,7 +160,7 @@ void testArm26() { for (int j = 0; j < state->getSize(); ++j) { stringstream message; message << "t=" << time <<" state# "<< j << " " << standard->getColumnLabels()[j+1] << " std=" << data[j] <<" computed=" << state->getData()[j] << endl; - ASSERT_EQUAL(data[j], state->getData()[j], 5.0e-3, + ASSERT_EQUAL(data[j], state->getData()[j], 5.0e-3, __FILE__, __LINE__, "ASSERT_EQUAL FAILED " + message.str()); cout << "ASSERT_EQUAL PASSED " << message.str(); } @@ -173,7 +173,7 @@ void testArm26() { for (int j = 0; j < state->getSize(); ++j) { stringstream message; message << "t=" << time <<" state# "<< j << " " << standard->getColumnLabels()[j+1] << " std=" << data[j] <<" computed=" << state->getData()[j] << endl; - ASSERT_EQUAL(data[j], state->getData()[j], 5.0e-3, + ASSERT_EQUAL(data[j], state->getData()[j], 5.0e-3, __FILE__, __LINE__, "ASSERT_EQUAL FAILED " + message.str()); cout << "ASSERT_EQUAL PASSED " << message.str(); } @@ -189,7 +189,7 @@ void testGait2354() Storage* standard = new Storage(); string statesFileName("std_subject01_walk1_states.sto"); forward.loadStatesStorage( statesFileName, standard ); - + int nstates = forward.getModel().getNumStateVariables(); int nq = forward.getModel().getNumCoordinates(); std::vector rms_tols(2*nstates, 0.001); //activations and fiber-lengths @@ -198,11 +198,12 @@ void testGait2354() rms_tols[2*i] = 0.035; // coordinates at less than 2degrees rms_tols[2*i+1] = 2.5; // speeds can deviate by a lot due to open-loop test } - - CHECK_STORAGE_AGAINST_STANDARD(results, *standard, rms_tols, + + CHECK_STORAGE_AGAINST_STANDARD(results, *standard, rms_tols, __FILE__, __LINE__, "testGait2354 failed"); } + void testGait2354WithController() { ForwardTool forward("subject01_Setup_Forward_Controller.xml"); forward.run(); @@ -220,7 +221,7 @@ void testGait2354WithController() { rms_tols[2*i] = 0.01; // coordinates at less than 0.6 degree rms_tols[2*i+1] = 0.1; // speeds should deviate less with feedback controller } - + CHECK_STORAGE_AGAINST_STANDARD(results, *standard, rms_tols, __FILE__, __LINE__, "testGait2354WithController failed"); } @@ -242,8 +243,6 @@ void testGait2354WithControllerGUI() { forward.updateModelForces(*model, ""); forward.setModel(*model); - model->initSystem(); - forward.run(); // For good measure we'll make sure we still get the identical results diff --git a/Bindings/Java/tests/TestModelBuilding.java b/Bindings/Java/tests/TestModelBuilding.java index fdf9cce6c9..8165af449f 100644 --- a/Bindings/Java/tests/TestModelBuilding.java +++ b/Bindings/Java/tests/TestModelBuilding.java @@ -32,14 +32,13 @@ public static void main(String[] args) { PrescribedController cns = new PrescribedController(); cns.addActuator(biceps); StepFunction testFunction = new StepFunction(0.5, 3.0, 0.3, 1.0); - cns.prescribeControlForActuator(0, //Index in controller set - testFunction); + cns.prescribeControlForActuator("biceps", testFunction); System.gc(); // Request gc could free testFunction and crash arm.addBody(hum); arm.addBody(rad); arm.addJoint(shoulder); arm.addJoint(elbow); - + arm.addForce(biceps); arm.addController(cns); /* diff --git a/Bindings/Python/tests/test_swig_additional_interface.py b/Bindings/Python/tests/test_swig_additional_interface.py index cb301d283e..0ab3c13a28 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/CHANGELOG.md b/CHANGELOG.md index 369fcb8e30..c4516818b7 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` @@ -24,6 +24,12 @@ new `getConnectee` overloads that take an index to a desired object in the list - Calling `getConnectee` no longer strongly requires that `finalizeConnection` has been called on the socket. The implementation will now fall back to the (slower) method of following the socket's connectee path property. This is useful if (e.g.) following sockets *during* a call to `Component::finalizeConnections` +- `Controller` now manages the list of controlled actuators using a list `Socket` instead of a `Set` (#3683). + The `actuator_list` property has been removed from `Controller` in lieu of the list `Socket`, which appears as + `socket_actuators` in the XML. This change also necessitated the addition of an added `initSystem()` call in + `AbstractTool::updateModelForces()` so that connected actuators have the same root component as the `Model` + at the time of `Socket` connection. Finally, `PrescribedController::prescribeControlForActuator(int, Function*)` is + now deprecated in favor of `PrescribedController::prescribeControlForActuator(const std::string&, Function*)`. v4.5 ==== diff --git a/OpenSim/Actuators/ModelFactory.cpp b/OpenSim/Actuators/ModelFactory.cpp index 021fb0fa6d..9c62626bc0 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. 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/Common/ComponentSocket.h b/OpenSim/Common/ComponentSocket.h index ce8f1ff88a..97d5617956 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. @@ -426,7 +426,7 @@ class Socket : public AbstractSocket { * connect to that component. * * Throws an exception If you provide only a component name and the - * model has multiple components with that nume. + * model has multiple components with that name. * */ void findAndConnect(const ComponentPath& connectee) override; 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/ExampleComponents/ToyReflexController.cpp b/OpenSim/ExampleComponents/ToyReflexController.cpp index ba03186819..275645e5e9 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 < (int)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 the 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 < (int)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/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/Moco/CMakeLists.txt b/OpenSim/Moco/CMakeLists.txt index 2d23eb4c5f..ad34b56d9c 100644 --- a/OpenSim/Moco/CMakeLists.txt +++ b/OpenSim/Moco/CMakeLists.txt @@ -82,14 +82,14 @@ set(MOCO_SOURCES MocoFrameDistanceConstraint.cpp MocoOutputConstraint.h MocoOutputConstraint.cpp - Components/DiscreteController.cpp - Components/DiscreteController.h Components/StationPlaneContactForce.h Components/StationPlaneContactForce.cpp Components/DiscreteForces.h Components/DiscreteForces.cpp Components/AccelerationMotion.h Components/AccelerationMotion.cpp + Components/ControlAllocator.h + Components/ControlAllocator.cpp MocoCasADiSolver/MocoCasADiSolver.h MocoCasADiSolver/MocoCasADiSolver.cpp MocoInverse.cpp diff --git a/OpenSim/Moco/Components/ControlAllocator.cpp b/OpenSim/Moco/Components/ControlAllocator.cpp new file mode 100644 index 0000000000..ceda09abf8 --- /dev/null +++ b/OpenSim/Moco/Components/ControlAllocator.cpp @@ -0,0 +1,104 @@ +/* -------------------------------------------------------------------------- * + * 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)]; +} + +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 +//============================================================================= +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..87979ee7d3 --- /dev/null +++ b/OpenSim/Moco/Components/ControlAllocator.h @@ -0,0 +1,100 @@ +#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; + + /** + * 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; + 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/Components/DiscreteController.cpp b/OpenSim/Moco/Components/DiscreteController.cpp deleted file mode 100644 index 5f7d44a4bf..0000000000 --- a/OpenSim/Moco/Components/DiscreteController.cpp +++ /dev/null @@ -1,62 +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 - -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(); - controls += discreteControls; -} - -void DiscreteController::extendRealizeTopology(SimTK::State& state) const { - Super::extendRealizeTopology(state); - const SimTK::Subsystem& subSys = getSystem().getDefaultSubsystem(); - m_discreteVarIndex = - subSys.allocateDiscreteVariable(state, SimTK::Stage::Dynamics, - new SimTK::Value( - SimTK::Vector(getModel().getNumControls(), 0.0))); -} diff --git a/OpenSim/Moco/Components/DiscreteController.h b/OpenSim/Moco/Components/DiscreteController.h deleted file mode 100644 index 09f44464e0..0000000000 --- a/OpenSim/Moco/Components/DiscreteController.h +++ /dev/null @@ -1,48 +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 - -#include - -namespace OpenSim { - -/** This component is used internally by Moco for passing a solver's control -variables to a Model. */ -class 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; - -}; - -} // namespace OpenSim - - -#endif // OPENSIM_DISCRETECONTROLLER_H diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.cpp index 78f40787ce..1a4b70c5db 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()); } @@ -76,7 +64,7 @@ MocoCasOCProblem::MocoCasOCProblem(const MocoCasADiSolver& mocoCasADiSolver, } auto controlNames = - createControlNamesFromModel(model, m_modelControlIndices); + problemRep.getControlAllocatorBase().getControlNamesInOrder(); for (const auto& controlName : controlNames) { const auto& info = problemRep.getControlInfo(controlName); addControl(controlName, convertBounds(info.getBounds()), diff --git a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h index 515203a46b..7e0c73096e 100644 --- a/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h +++ b/OpenSim/Moco/MocoCasADiSolver/MocoCasOCProblem.h @@ -22,8 +22,8 @@ #include "MocoCasADiSolver.h" #include -#include #include +#include #include #include @@ -345,9 +345,10 @@ class MocoCasOCProblem : public CasOC::Problem { auto& simtkStateDisabledConstraints = mocoProblemRep->updStateDisabledConstraints(); - const auto& discreteController = - mocoProblemRep->getDiscreteControllerDisabledConstraints(); - const auto& rawControls = discreteController.getDiscreteControls( + const auto& controlAllocatorDisabledConstraints = + mocoProblemRep->getControlAllocatorDisabledConstraints(); + const auto& rawControls = + controlAllocatorDisabledConstraints.getControls( simtkStateDisabledConstraints); integrand = mocoCost.calcIntegrand( @@ -376,11 +377,13 @@ class MocoCasOCProblem : public CasOC::Problem { auto& simtkStateDisabledConstraintsFinal = mocoProblemRep->updStateDisabledConstraints(1); - const auto& discreteController = - mocoProblemRep->getDiscreteControllerDisabledConstraints(); - const auto& rawControlsInitial = discreteController.getDiscreteControls( + const auto& controlAllocatorDisabledConstraints = + mocoProblemRep->getControlAllocatorDisabledConstraints(); + const auto& rawControlsInitial = + controlAllocatorDisabledConstraints.getControls( simtkStateDisabledConstraintsInitial); - const auto& rawControlsFinal = discreteController.getDiscreteControls( + const auto& rawControlsFinal = + controlAllocatorDisabledConstraints.getControls( simtkStateDisabledConstraintsFinal); // Compute the cost for this cost term. @@ -410,9 +413,10 @@ class MocoCasOCProblem : public CasOC::Problem { auto& simtkStateDisabledConstraints = mocoProblemRep->updStateDisabledConstraints(); - const auto& discreteController = - mocoProblemRep->getDiscreteControllerDisabledConstraints(); - const auto& rawControls = discreteController.getDiscreteControls( + const auto& controlAllocatorDisabledConstraints = + mocoProblemRep->getControlAllocatorDisabledConstraints(); + const auto& rawControls = + controlAllocatorDisabledConstraints.getControls( simtkStateDisabledConstraints); integrand = mocoEC.calcIntegrand( @@ -442,11 +446,13 @@ class MocoCasOCProblem : public CasOC::Problem { auto& simtkStateDisabledConstraintsFinal = mocoProblemRep->updStateDisabledConstraints(1); - const auto& discreteController = - mocoProblemRep->getDiscreteControllerDisabledConstraints(); - const auto& rawControlsInitial = discreteController.getDiscreteControls( + const auto& controlAllocatorDisabledConstraints = + mocoProblemRep->getControlAllocatorDisabledConstraints(); + const auto& rawControlsInitial = + controlAllocatorDisabledConstraints.getControls( simtkStateDisabledConstraintsInitial); - const auto& rawControlsFinal = discreteController.getDiscreteControls( + const auto& rawControlsFinal = + controlAllocatorDisabledConstraints.getControls( simtkStateDisabledConstraintsFinal); // Compute the cost for this cost term. @@ -553,15 +559,15 @@ 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); + 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); } } } @@ -622,7 +628,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 @@ -750,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/MocoControlBoundConstraint.cpp b/OpenSim/Moco/MocoControlBoundConstraint.cpp index 03df7deea7..dd2fa95dc6 100644 --- a/OpenSim/Moco/MocoControlBoundConstraint.cpp +++ b/OpenSim/Moco/MocoControlBoundConstraint.cpp @@ -22,6 +22,7 @@ #include #include +#include using namespace OpenSim; @@ -34,17 +35,19 @@ void MocoControlBoundConstraint::constructProperties() { constructProperty_lower_bound(); constructProperty_upper_bound(); constructProperty_equality_with_lower(false); + constructProperty_ignore_controlled_actuators(false); } 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); + // 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) { @@ -59,8 +62,21 @@ void MocoControlBoundConstraint::initializeOnModelImpl( OPENSIM_THROW_IF_FRMOBJ(systemControlIndexMap.count(thisName) == 0, Exception, "Control path '{}' was provided but no such " - "control exists in the model.", - thisName); + "control exists in the model or it is already controlled " + "by a user-defined controller.", + 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 032bb6c76d..c82b7242d9 100644 --- a/OpenSim/Moco/MocoControlBoundConstraint.h +++ b/OpenSim/Moco/MocoControlBoundConstraint.h @@ -34,10 +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. + @ingroup mocopathcon */ class OSIMMOCO_API MocoControlBoundConstraint : public MocoPathConstraint { OpenSim_DECLARE_CONCRETE_OBJECT( @@ -84,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; @@ -102,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 1b0201d128..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( @@ -57,6 +59,10 @@ void MocoControlGoal::initializeOnModelImpl(const Model& model) const { // Check that the model controls are in the correct order. checkOrderSystemControls(model); + // 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) { @@ -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 35981bfc4a..ea4bef4667 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. + +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 { OpenSim_DECLARE_CONCRETE_OBJECT(MocoControlGoal, MocoGoal); @@ -86,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( @@ -102,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 8cd4cb24bd..c2c742fb6d 100644 --- a/OpenSim/Moco/MocoGoal/MocoControlTrackingGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoControlTrackingGoal.cpp @@ -22,6 +22,7 @@ #include #include +#include using namespace OpenSim; @@ -69,13 +70,18 @@ void MocoControlTrackingGoal::initializeOnModelImpl(const Model& model) const { // 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) { const auto& weightName = get_control_weights().get(i).getName(); 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 +98,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 " @@ -148,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 6d41054ea9..f3b737dd98 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. + +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 { OpenSim_DECLARE_CONCRETE_OBJECT(MocoControlTrackingGoal, MocoGoal); @@ -231,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: @@ -273,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 f7c947b43b..dccca44bf4 100644 --- a/OpenSim/Moco/MocoGoal/MocoInitialActivationGoal.cpp +++ b/OpenSim/Moco/MocoGoal/MocoInitialActivationGoal.cpp @@ -19,22 +19,24 @@ #include "MocoInitialActivationGoal.h" #include +#include 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); + + // Get a map of all the control indices in the system. auto systemControlIndexMap = createSystemControlIndexMap(model); for (const auto& muscle : model.getComponentList()) { - if (!muscle.get_ignore_activation_dynamics()) { - const std::string path = muscle.getAbsolutePathString(); - int excitationIndex = systemControlIndexMap[path]; - int activationIndex = allSysYIndices[path + "/activation"]; - m_indices.emplace_back(excitationIndex, activationIndex); - } + const std::string path = muscle.getAbsolutePathString(); + 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 12d9dfdd48..5f6fd30b01 100644 --- a/OpenSim/Moco/MocoGoal/MocoInitialActivationGoal.h +++ b/OpenSim/Moco/MocoGoal/MocoInitialActivationGoal.h @@ -30,6 +30,7 @@ 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. + @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..993222a217 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; @@ -57,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 { @@ -79,15 +81,39 @@ void MocoPeriodicityGoal::initializeOnModelImpl(const Model& model) const { } auto systemControlIndexMap = createSystemControlIndexMap(model); - int nControlPairs = getProperty_control_pairs().size(); + // 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 '{}'.", path1); + "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 '{}'.", path2); + "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); @@ -97,7 +123,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( @@ -114,8 +140,10 @@ void MocoPeriodicityGoal::calcGoalImpl( ++i; } - const auto& initialControls = input.initial_controls; - const auto& finalControls = input.final_controls; + 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) { goal[i + j] = (initialControls[std::get<0>(index_control)] * diff --git a/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.h b/OpenSim/Moco/MocoGoal/MocoPeriodicityGoal.h index 6e7faec96e..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 @@ -95,6 +99,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); @@ -120,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 { @@ -135,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/MocoProblemRep.cpp b/OpenSim/Moco/MocoProblemRep.cpp index 68e98c298e..a637d3c6e6 100644 --- a/OpenSim/Moco/MocoProblemRep.cpp +++ b/OpenSim/Moco/MocoProblemRep.cpp @@ -19,8 +19,8 @@ #include "MocoProblemRep.h" #include "Components/AccelerationMotion.h" -#include "Components/DiscreteController.h" #include "Components/DiscreteForces.h" +#include "Components/ControlAllocator.h" #include "MocoProblem.h" #include "MocoProblemInfo.h" #include "MocoScaleFactor.h" @@ -29,6 +29,8 @@ #include #include +#include +#include using namespace OpenSim; @@ -94,9 +96,88 @@ void MocoProblemRep::initialize() { } } - auto discreteControllerBaseUPtr = make_unique(); - m_discrete_controller_base.reset(discreteControllerBaseUPtr.get()); - m_model_base.addController(discreteControllerBaseUPtr.release()); + // 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); + + // 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 " + "components. Controller '{}' is of type " + "'{}'.", + controller.getAbsolutePathString(), + controller.getConcreteClassName()); + } + const auto& socket = controller.getSocket("actuators"); + for (int i = 0; i < static_cast(socket.getNumConnectees()); ++i) { + controlledActuatorPaths.push_back(socket.getConnecteePath(i)); + } + } + + // Add the non-controlled, enabled actuators to an ActuatorInputController. + auto actuatorController = make_unique(); + 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()) { + actuatorController->addActuator(actu); + } + } + 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 + // 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()) { + auto& socket = controller.updSocket("actuators"); + 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) { + 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); + } + } 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 // ------------- @@ -177,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 @@ -187,8 +269,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) { @@ -428,15 +510,19 @@ void MocoProblemRep::initialize() { // Control infos. // -------------- - auto controlNames = createControlNamesFromModel(m_model_base); + // 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); 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]] = - ph0.get_control_infos_pattern(i); - m_control_infos[controlNames[j]].setName(controlNames[j]); + 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); } } } @@ -445,8 +531,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); } @@ -455,9 +541,13 @@ 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 the associated actuator control variables. - for (const auto& actu : m_model_base.getComponentList()) { + // 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.getComponentList().begin(); + const auto& socket = actuController->getSocket("actuators"); + 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) { // No control info exists; add one. @@ -479,19 +569,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. @@ -509,6 +586,28 @@ void MocoProblemRep::initialize() { } } + // 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(); + 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 bd64ca49eb..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,7 @@ namespace OpenSim { class MocoProblem; -class DiscreteController; +class ControlAllocator; class DiscreteForces; class PositionMotion; class AccelerationMotion; @@ -55,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. @@ -89,8 +89,8 @@ 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(); } /// Get a reference to a copy of the model being used by this /// MocoProblemRep, but with all constraints disabled and an additional @@ -117,8 +117,8 @@ 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(); } /// This is a component inside ModelDisabledConstraints that you can use /// to set the value of discrete forces, intended to hold the constraint @@ -321,7 +321,6 @@ class OSIMMOCO_API MocoProblemRep { getImplicitComponentReferencePtrs() const { return m_implicit_component_refs; } - /// @} private: explicit MocoProblemRep(const MocoProblem& problem); @@ -380,13 +379,13 @@ 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 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/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/Moco/Test/testMocoInterface.cpp b/OpenSim/Moco/Test/testMocoInterface.cpp index a844f8e75c..63e41dfea1 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. @@ -2163,7 +2164,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", @@ -2233,26 +2234,61 @@ TEMPLATE_TEST_CASE("Locked coordinates ", "", ContainsSubstring("Coordinate '/slider/position' is locked")); } -/* -TEMPLATE_TEST_CASE("Controllers in the model", "", +TEMPLATE_TEST_CASE("Sliding mass with PrescribedController", "", MocoCasADiSolver, MocoTropterSolver) { - 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, 0); - problem.addGoal(); - auto& solver = study.initSolver(); - solver.set_num_mesh_points(20); - 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(); + 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("/forceset/actuator", {-50, 50}); + problem.addGoal(); + 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(); + } + + // 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("/forceset/actuator"); + Model model = ModelFactory::createSlidingPointMass(); + auto* controller = new PrescribedController(); + controller->addActuator( + model.getComponent("/forceset/actuator")); + controller->prescribeControlForActuator("/forceset/actuator", + new GCVSpline(5, control.size(), time.data(), &control[0], + "/forceset/actuator", 0.0)); + model.addController(controller); + model.finalizeConnections(); + MocoStudy study; + auto& problem = study.updProblem(); + 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); + } } -*/ 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/Moco/tropter/TropterProblem.h b/OpenSim/Moco/tropter/TropterProblem.h index 1ba0421385..714aa1af61 100644 --- a/OpenSim/Moco/tropter/TropterProblem.h +++ b/OpenSim/Moco/tropter/TropterProblem.h @@ -21,8 +21,8 @@ #include #include -#include #include +#include #include #include #include @@ -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. @@ -109,8 +96,10 @@ class MocoTropterSolver::TropterProblemBase : public tropter::Problem { void addControlVariables() { auto controlNames = - createControlNamesFromModel(m_modelBase, m_modelControlIndices); + 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()), @@ -339,10 +328,10 @@ 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[m_modelControlIndices[ic]] = controls[ic]; + osimControls[ic] = controls[ic]; } } @@ -375,9 +364,10 @@ 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& controlAllocatorDisabledConstraints = + m_mocoProbRep.getControlAllocatorDisabledConstraints(); + const auto& rawControls = + controlAllocatorDisabledConstraints.getControls( this->m_stateDisabledConstraints); // Compute the integrand for this cost term. @@ -400,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& discreteController = - m_mocoProbRep.getDiscreteControllerDisabledConstraints(); - const auto& initialRawControls = discreteController.getDiscreteControls( - initialState); - const auto& finalRawControls = discreteController.getDiscreteControls( - 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/Sandbox/Moco/sandboxSandbox.cpp b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp index d935990b59..8e30852df9 100644 --- a/OpenSim/Sandbox/Moco/sandboxSandbox.cpp +++ b/OpenSim/Sandbox/Moco/sandboxSandbox.cpp @@ -19,19 +19,11 @@ // This file provides a way to easily prototype or test temporary snippets of // code during development. -#include +#include 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; } diff --git a/OpenSim/Simulation/Control/ControlSetController.cpp b/OpenSim/Simulation/Control/ControlSetController.cpp index 553aeecc23..ee67fc5afd 100644 --- a/OpenSim/Simulation/Control/ControlSetController.cpp +++ b/OpenSim/Simulation/Control/ControlSetController.cpp @@ -29,13 +29,14 @@ //============================================================================= // INCLUDES //============================================================================= -#include "Controller.h" #include "ControlSetController.h" #include "ControlLinear.h" #include "ControlSet.h" -#include -#include +#include "Controller.h" +#include +#include +#include //============================================================================= // STATICS @@ -162,13 +163,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 +179,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); } } } @@ -260,15 +262,51 @@ 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()); } - if (getProperty_actuator_list().findIndex(actName) < 0) // not already in the list of actuators for this controller - updProperty_actuator_list().appendValue(actName); + + // 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) { + 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()); } } + diff --git a/OpenSim/Simulation/Control/ControlSetController.h b/OpenSim/Simulation/Control/ControlSetController.h index 332fc3f920..365ea83639 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 the ControlSet. void extendFinalizeFromProperties() override; + /// Update the Controller's actuator list. + void extendConnectToModel(Model& model) override; + //-------------------------------------------------------------------------- // OPERATORS //-------------------------------------------------------------------------- diff --git a/OpenSim/Simulation/Control/Controller.cpp b/OpenSim/Simulation/Control/Controller.cpp index 72a27b1741..2e517f0f01 100644 --- a/OpenSim/Simulation/Control/Controller.cpp +++ b/OpenSim/Simulation/Control/Controller.cpp @@ -27,82 +27,43 @@ #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); } +//============================================================================= +// 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"}; @@ -118,110 +79,87 @@ 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); } -//============================================================================= -// GET AND SET -//============================================================================= - -//----------------------------------------------------------------------------- -// ON/OFF -//----------------------------------------------------------------------------- -//_____________________________________________________________________________ -/** - * Get whether or not this controller is enabled. - */ -bool Controller::isEnabled() const -{ - return get_enabled(); -} -//_____________________________________________________________________________ -/** - * Turn this controller on or off. - */ -void Controller::setEnabled(bool aTrueFalse) -{ - upd_enabled() = aTrueFalse; -} - -// for any post XML deserialization initialization -void Controller::extendConnectToModel(Model& model) -{ +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; + // 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) { + // 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. + addActuator(actuator); break; } - } - if (!found) { - cerr << "WARN: Controller::connectToModel : Actuator " - << get_actuator_list(i) << - " was not found and will be ignored." << endl; } } + // Call finalizeConnection() to sync the connectee path names with the + // connected Actuators. + socket.finalizeConnection(model); + _actuatorNamesFromXML.clear(); } } -/** - * Create a Controller in the SimTK::System - */ -void Controller::extendAddToSystem(SimTK::MultibodySystem& system) const -{ - Super::extendAddToSystem(system); +//============================================================================= +// GET AND SET +//============================================================================= +bool Controller::isEnabled() const { + return get_enabled(); } -// 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::setEnabled(bool aTrueFalse) { + upd_enabled() = aTrueFalse; } +void Controller::setActuators(const Set& actuators) { + updSocket("actuators").disconnect(); + for (int i = 0; i < actuators.getSize(); i++){ + addActuator(actuators.get(i)); + } +} -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::setActuators(const ComponentList& actuators) { + updSocket("actuators").disconnect(); + for (const auto& actu : actuators) { + addActuator(actu); + } } -Set& Controller::updActuators() { return _actuatorSet; } - -const Set& Controller::getActuatorSet() const { return _actuatorSet; } +void Controller::addActuator(const Actuator& actuator) { + appendSocketConnectee_actuators(actuator); +} diff --git a/OpenSim/Simulation/Control/Controller.h b/OpenSim/Simulation/Control/Controller.h index 1c1d277e50..8525fe21d9 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; @@ -41,13 +35,38 @@ 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() * - * @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. + * 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()); + * @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 */ @@ -55,21 +74,19 @@ 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**. - 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." ); - 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 +94,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 +116,12 @@ 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 ComponentList& 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(); /** Compute the control for actuator * This method defines the behavior for any concrete controller @@ -120,41 +133,42 @@ OpenSim_DECLARE_ABSTRACT_OBJECT(Controller, ModelComponent); virtual void computeControls(const SimTK::State& s, SimTK::Vector &controls) const = 0; - 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. */ - void extendConnectToModel(Model& model) override; + /** Get the number of controls this controller computes. */ + int getNumControls() const { return _numControls; } - /** 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; + /** Get the number of actuators that this controller is connected to. */ + int getNumActuators() const { + return static_cast( + getSocket("actuators").getNumConnectees()); + } - /** Only a Controller can set its number of controls based on its actuators */ - void setNumControls(int numControls) {_numControls = numControls; } +protected: + /** Only a Controller can set its number of controls based on its + * 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: - // 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 + // 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; -}; //namespace -//============================================================================= -//============================================================================= +}; // class Controller + +} // namespace OpenSim #endif // OPENSIM_CONTROLLER_H_ diff --git a/OpenSim/Simulation/Control/InputController.cpp b/OpenSim/Simulation/Control/InputController.cpp new file mode 100644 index 0000000000..e5abaa4ce3 --- /dev/null +++ b/OpenSim/Simulation/Control/InputController.cpp @@ -0,0 +1,185 @@ +/* -------------------------------------------------------------------------- * + * 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) = default; + +InputController& +InputController::operator=(const InputController& other) = default; + +InputController::InputController(InputController&& other) = default; + +InputController& InputController::operator=(InputController&& other) = 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("controls"); + aliases.reserve(input.getNumConnectees()); + for (int i = 0; i < static_cast(input.getNumConnectees()); ++i) { + aliases.push_back(input.getAlias(i)); + } + + return aliases; +} + +//============================================================================= +// MODEL COMPONENT INTERFACE +//============================================================================= +void InputController::extendConnectToModel(Model& model) { + 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& socket = getSocket("actuators"); + for (int i = 0; i < static_cast(socket.getNumConnectees()); ++i) { + const auto& actu = socket.getConnectee(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) = default; + +ActuatorInputController& ActuatorInputController::operator=( + const ActuatorInputController& other) = default; + +ActuatorInputController::ActuatorInputController( + ActuatorInputController&& other) = default; + +ActuatorInputController& ActuatorInputController::operator=( + ActuatorInputController&& other) = default; + +//============================================================================= +// CONTROLLER INTERFACE +//============================================================================= +void ActuatorInputController::computeControls(const SimTK::State& s, + SimTK::Vector& controls) const { + const auto& input = getInput("controls"); + for (int i = 0; i < static_cast(input.getNumConnectees()); ++i) { + controls[m_controlIndexesInConnecteeOrder[i]] = input.getValue(s, i); + } +} + +//============================================================================= +// MODEL COMPONENT INTERFACE +//============================================================================= +void ActuatorInputController::extendConnectToModel(Model& model) { + Super::extendConnectToModel(model); + + const auto& controlNames = getControlNames(); + const auto& controlIndexes = getControlIndexes(); + OPENSIM_ASSERT(getNumControls() == + static_cast(controlNames.size())) + + const auto& input = getInput("controls"); + 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()); + + // 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()]); + } +} diff --git a/OpenSim/Simulation/Control/InputController.h b/OpenSim/Simulation/Control/InputController.h new file mode 100644 index 0000000000..a02c71e081 --- /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, "TODO"); + +//============================================================================= +// METHODS +//============================================================================= + + // CONSTRUCTION AND DESTRUCTION + InputController(); + ~InputController() override; + + InputController(const InputController& other); + InputController& operator=(const InputController& other); + + InputController(InputController&& other); + InputController& operator=(InputController&& other); + + // 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); + ActuatorInputController& operator=(ActuatorInputController&& other); + + // 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..84d72d9e65 100644 --- a/OpenSim/Simulation/Control/PrescribedController.cpp +++ b/OpenSim/Simulation/Control/PrescribedController.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 * @@ -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,127 +63,187 @@ 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__); - } + 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. + 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(); - - 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; + 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 (!_actuLabelsToControlFunctionIndexMap.count(columnLabel)) { + + // See if the column label matches a connected actuator. If not, + // find and add the actuator to the controller. + int actuIndex = getActuatorIndexFromLabel(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); } - 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()); - if(inC >= 0) - prescribeControlForActuator(inC, pfunc); - else{ // add the actuator to the controller's list - updProperty_actuator_list().appendValue(actuator->getName()); - controllerActuators.adoptAndAppend(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 -} + // Create the control function and assign it to the actuator. + controls.getDataColumn( + controls.getStateIndex(columnLabel), data); + Function* controlFunction = createFunctionFromData(columnLabel, + time, data); + 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() != (int)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()); - for(int i=0; i("actuators"); + for(int i = 0; i < (int)socket.getNumConnectees(); ++i){ actControls[0] = get_ControlFunctions()[i].calcValue(time); - getActuatorSet()[i].addInControls(actControls, controls); - } + socket.getConnectee(i).addInControls(actControls, controls); + } } - //============================================================================= // 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." ); - OPENSIM_THROW_IF(index >= getActuatorSet().getSize(), - IndexOutOfRange, (size_t)index, 0, - (size_t)getActuatorSet().getSize() - 1); - - 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 (_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; + } } -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."); - prescribeControlForActuator(index, 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 < (int)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..f43c9766c8 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-2024 Stanford University and the Authors * * Author(s): Ajay Seth * * * * Licensed under the Apache License, Version 2.0 (the "License"); you may * @@ -31,36 +31,34 @@ 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. * + * @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 { 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 +66,8 @@ OpenSim_DECLARE_CONCRETE_OBJECT(PrescribedController, Controller); //============================================================================= // METHODS //============================================================================= - //-------------------------------------------------------------------------- + // CONSTRUCTION AND DESTRUCTION - //-------------------------------------------------------------------------- -public: /** Default constructor */ PrescribedController(); @@ -84,11 +80,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 +93,49 @@ 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 + * Assign a prescribed control function for the desired actuator identified + * 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(int index, Function *prescribedFunction); + void prescribeControlForActuator(const std::string& actuLabel, + 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 - */ - void prescribeControlForActuator(const std::string actName, - 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 */ + // 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 + // Member variables. + std::unordered_map _actuLabelsToControlFunctionIndexMap; + std::unordered_map _actuIndexToControlFunctionIndexMap; -}; //namespace -//============================================================================= -//============================================================================= +}; // class PrescribedController + +} // namespace OpenSim #endif // OPENSIM_PRESCRIBED_CONTROLLER_H_ 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/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;j #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() ); diff --git a/OpenSim/Simulation/SimulationUtilities.cpp b/OpenSim/Simulation/SimulationUtilities.cpp index 4b1bbb3b54..6b0adda007 100644 --- a/OpenSim/Simulation/SimulationUtilities.cpp +++ b/OpenSim/Simulation/SimulationUtilities.cpp @@ -308,11 +308,11 @@ std::unordered_map OpenSim::createSystemYIndexMap( std::vector OpenSim::createControlNamesFromModel( const Model& model, std::vector& modelControlIndices) { std::vector controlNames; + // 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()) { @@ -336,6 +336,7 @@ std::vector OpenSim::createControlNamesFromModel( return controlNames; } + std::vector OpenSim::createControlNamesFromModel( const Model& model) { std::vector modelControlIndices; @@ -351,7 +352,6 @@ 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; const SimTK::State state = model.getWorkingState(); auto modelControls = model.updControls(state); diff --git a/OpenSim/Simulation/SimulationUtilities.h b/OpenSim/Simulation/SimulationUtilities.h index 2dd9fb52c7..e50f0f0930 100644 --- a/OpenSim/Simulation/SimulationUtilities.h +++ b/OpenSim/Simulation/SimulationUtilities.h @@ -164,13 +164,61 @@ std::unordered_map createSystemYIndexMap(const Model& model); 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.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 < static_cast(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. +/// /// @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 diff --git a/OpenSim/Simulation/Test/testInitState.cpp b/OpenSim/Simulation/Test/testInitState.cpp index 5a5aefd364..3d838f24c8 100644 --- a/OpenSim/Simulation/Test/testInitState.cpp +++ b/OpenSim/Simulation/Test/testInitState.cpp @@ -71,7 +71,7 @@ void testStates(const string& modelFile) Model model(modelFile); ControlSetController* controller = new ControlSetController(); controller->setControlSetFileName("arm26_StaticOptimization_controls.xml"); - + model.addController( controller ); // original default state State& state = model.initSystem(); 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/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 f76a223525..71a0104297 100644 --- a/OpenSim/Tests/Wrapping/testWrapping.cpp +++ b/OpenSim/Tests/Wrapping/testWrapping.cpp @@ -82,10 +82,13 @@ namespace { // Create a PrescribedController that simply applies a function of the // force. PrescribedController actuController; - actuController.setActuators(model.updActuators()); - for (int i = 0; i < actuController.getActuatorSet().getSize(); i++) { + const auto& actuatorsSet = model.getActuators(); + actuController.setActuators(actuatorsSet); + const auto& socket = actuController.getSocket("actuators"); + for (int i = 0; i < (int)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 @@ -373,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(); @@ -442,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(); @@ -542,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(); diff --git a/OpenSim/Tools/ActuatorForceTarget.cpp b/OpenSim/Tools/ActuatorForceTarget.cpp index 6a95c9896f..4087c3f293 100644 --- a/OpenSim/Tools/ActuatorForceTarget.cpp +++ b/OpenSim/Tools/ActuatorForceTarget.cpp @@ -123,7 +123,7 @@ prepareToOptimize(SimTK::State& s, double *x) _saveState = s; #ifdef USE_PRECOMPUTED_PERFORMANCE_MATRICES // int nu = _controller->getModel().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..d65a4dfeb8 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..fe02b950d9 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 < (int)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..c720309c11 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 = + getActuatorsPathsForCMC(_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); @@ -541,7 +555,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 @@ -1089,44 +1103,86 @@ 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); +std::vector CMCTool::getActuatorsPathsForCMC( + 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(); + 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()) { + continue; + } + 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 actuatorsForCMC; + return actuatorsForCMCPaths; } diff --git a/OpenSim/Tools/CMCTool.h b/OpenSim/Tools/CMCTool.h index 3cecf73e0f..da0e5667a1 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 vector of model actuator paths for CMC that exclude user + specified Actuators */ + std::vector getActuatorsPathsForCMC( + const Array &actuatorsByNameOrGroup); //-------------------------------------------------------------------------- // OPERATORS diff --git a/OpenSim/Tools/CorrectionController.cpp b/OpenSim/Tools/CorrectionController.cpp index 88002d1f53..25aad2eefd 100644 --- a/OpenSim/Tools/CorrectionController.cpp +++ b/OpenSim/Tools/CorrectionController.cpp @@ -291,7 +291,9 @@ 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. + setNumControls(getNumActuators()); log_info("CorrectionController::extendConnectToModel(): " "numActuators = {:d}, kv = {:0.3f}, kp = {:0.3f}", diff --git a/OpenSim/Tools/Test/testControllers.cpp b/OpenSim/Tools/Test/testControllers.cpp index 7cf00a7b63..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-2017 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 * @@ -26,53 +26,50 @@ // 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 // //============================================================================= #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; +TEST_CASE("Test Controller interface") { + + Model model = ModelFactory::createNLinkPendulum(2); + auto* controller = new PrescribedController(); + controller->prescribeControlForActuator("/tau0", new Constant(1.0)); + controller->prescribeControlForActuator("/tau1", new Constant(2.0)); + + SECTION("No actuators connecteed") { + model.addController(controller); + REQUIRE_THROWS(model.finalizeConnections()); + } + + SECTION("Adding actuators individually") { + controller->addActuator(model.getComponent("/tau0")); + controller->addActuator(model.getComponent("/tau1")); + model.addController(controller); + model.finalizeConnections(); + } + + SECTION("Adding multiple actuators from a ComponentList") { + controller->setActuators(model.getComponentList()); + model.addController(controller); + model.finalizeConnections(); } - log_info("TestControllers passed."); - return 0; } -//========================================================================================================== -void testControlSetControllerOnBlock() -{ +TEST_CASE("testControlSetControllerOnBlock") { using namespace SimTK; // Create a new OpenSim model @@ -125,13 +122,15 @@ 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(); + // 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(); @@ -162,14 +161,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"); @@ -214,7 +213,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 @@ -258,12 +259,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 @@ -310,15 +309,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; @@ -336,7 +336,6 @@ void testPrescribedControllerFromFile(const std::string& modelFile, ControlSetController csc; ControlSet* cs = new ControlSet(controlsFile); csc.setControlSet(cs); - // add the controller to the model osimModel.addController(&csc); diff --git a/OpenSim/Tools/VectorFunctionForActuators.cpp b/OpenSim/Tools/VectorFunctionForActuators.cpp index 894acf00ce..9d3409cc88 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,11 @@ evaluate(const SimTK::State& s, const double *aX, double *rF) ts.initialize(actSysState); ts.stepTo(_tf); - const Set& forceSet = controller.getActuatorSet(); // Vector function values + const auto& socket = controller.getSocket("actuators"); 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++; }