1 /* -------------------------------------------------------------------------- *
2 * OpenSim: Actuator.cpp *
3 * -------------------------------------------------------------------------- *
4 * The OpenSim API is a toolkit for musculoskeletal modeling and simulation. *
5 * See http://opensim.stanford.edu and the NOTICE file for more information. *
6 * OpenSim is developed at Stanford University and supported by the US *
7 * National Institutes of Health (U54 GM072970, R24 HD065690) and by DARPA *
8 * through the Warrior Web program. *
9 * *
10 * Copyright (c) 2005-2017 Stanford University and the Authors *
11 * Author(s): Ajay Seth *
12 * *
13 * Licensed under the Apache License, Version 2.0 (the "License"); you may *
14 * not use this file except in compliance with the License. You may obtain a *
15 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0. *
16 * *
17 * Unless required by applicable law or agreed to in writing, software *
18 * distributed under the License is distributed on an "AS IS" BASIS, *
19 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. *
20 * See the License for the specific language governing permissions and *
21 * limitations under the License. *
22 * -------------------------------------------------------------------------- */
23
24 //=============================================================================
25 // INCLUDES
26 //=============================================================================
27 #include <OpenSim/Simulation/Model/Model.h>
28 #include "Actuator.h"
29 #include "OpenSim/Common/DebugUtilities.h"
30
31
32 using namespace std;
33 using namespace OpenSim;
34 using namespace SimTK;
35
36 //=============================================================================
37 // CONSTRUCTOR(S) AND DESTRUCTOR
38 //=============================================================================
39 //_____________________________________________________________________________
40 /**
41 * Construct an actuator of controls.
42 * This is the base class which generates a vector of actuator controls.
43 *
44 */
Actuator()45 Actuator::Actuator()
46 {
47 setNull();
48 }
49
50
51 //=============================================================================
52 // CONSTRUCTION METHODS
53 //=============================================================================
54 //_____________________________________________________________________________
55 /**
56 * Set the data members of this Actuator to their null values.
57 */
setNull()58 void Actuator::setNull()
59 {
60 setAuthors("Ajay Seth");
61 _controlIndex = -1;
62 }
63
64 // Create the underlying computational system component(s) that support the
65 // Actuator model component
extendAddToSystem(SimTK::MultibodySystem & system) const66 void Actuator::extendAddToSystem(SimTK::MultibodySystem& system) const
67 {
68 Super::extendAddToSystem(system);
69 // Beyond the const Component get the index so we can access the SimTK::Force later
70 Actuator* mutableThis = const_cast<Actuator *>(this);
71
72 // Model is in charge of creating the shared cache for all actuator controls
73 // but it does so based on the size and order in its _defaultControls
74 // Actuator has the opportunity here to add slots for its control and record
75 // the index into the shared cache Vector.
76 mutableThis->_controlIndex = _model->updDefaultControls().size();
77 _model->updDefaultControls().resizeKeep(_controlIndex + numControls());
78 _model->updDefaultControls()(_controlIndex, numControls()) = Vector(numControls(), 0.0);
79 }
80
81 // CONTROLS
82 //_____________________________________________________________________________
83 /**
84 * Get the actuator's control values
85 *
86 * @param the current state
87 * @return The value of the controls.
88 */
getControls(const SimTK::State & s) const89 const VectorView_<Real> Actuator::getControls( const SimTK::State& s ) const
90 {
91 const Vector &controlsCache = _model->getControls(s);
92 return controlsCache(_controlIndex, numControls());
93 }
94
getControls(const Vector & modelControls,Vector & actuatorControls) const95 void Actuator::getControls(const Vector& modelControls, Vector& actuatorControls) const
96 {
97 SimTK_ASSERT(modelControls.size() == _model->getNumControls(),
98 "Actuator::getControls, input modelControls size does not match model.getNumControls().\n");
99 SimTK_ASSERT(actuatorControls.size() == numControls(),
100 "Actuator::getControls, output actuatorControls incompatible with actuator's numControls().\n");
101 actuatorControls = modelControls(_controlIndex, numControls());
102 }
103
setControls(const Vector & actuatorControls,Vector & modelControls) const104 void Actuator::setControls(const Vector& actuatorControls, Vector& modelControls) const
105 {
106 SimTK_ASSERT(actuatorControls.size() == numControls(),
107 "Actuator::setControls, input actuatorControls incompatible with actuator's numControls().\n");
108
109 SimTK_ASSERT(modelControls.size() == _model->getNumControls(),
110 "Actuator::setControls, output modelControls size does not match model.getNumControls()\n");
111
112 modelControls(_controlIndex, numControls()) = actuatorControls;
113 }
114
addInControls(const Vector & actuatorControls,Vector & modelControls) const115 void Actuator::addInControls(const Vector& actuatorControls, Vector& modelControls) const
116 {
117 SimTK_ASSERT(actuatorControls.size() == numControls(),
118 "Actuator::addInControls, input actuatorControls incompatible with actuator's numControls()\n");
119
120 SimTK_ASSERT(modelControls.size() == _model->getNumControls(),
121 "Actuator::addInControls, output modelControls size does not match model.getNumControls().\n");
122
123 modelControls(_controlIndex, numControls()) += actuatorControls;
124 }
125
126
127
128
129 //=============================================================================
130 // Scalar Actuator Implementation
131 //=============================================================================
132 //_____________________________________________________________________________
133
134 /** Default constructor */
ScalarActuator()135 ScalarActuator::ScalarActuator()
136 {
137 constructProperties();
138 }
139
140 /**
141 * Set up the serializable member variables. This involves constructing and
142 * initializing properties.
143 */
constructProperties()144 void ScalarActuator::constructProperties()
145 {
146 constructProperty_min_control(-Infinity);
147 constructProperty_max_control( Infinity);
148 }
149
setMinControl(const double & aMinControl)150 void ScalarActuator::setMinControl(const double& aMinControl)
151 {
152 set_min_control(aMinControl);
153 }
154
getMinControl() const155 double ScalarActuator::getMinControl() const
156 {
157 return get_min_control();
158 }
159
setMaxControl(const double & aMaxControl)160 void ScalarActuator::setMaxControl(const double& aMaxControl)
161 {
162 set_max_control(aMaxControl);
163 }
164
getMaxControl() const165 double ScalarActuator::getMaxControl() const
166 {
167 return get_max_control();
168 }
169
170 // Create the underlying computational system component(s) that support the
171 // ScalarActuator model component
extendAddToSystem(SimTK::MultibodySystem & system) const172 void ScalarActuator::extendAddToSystem(SimTK::MultibodySystem& system) const
173 {
174 Super::extendAddToSystem(system);
175 // Add modeling flag to compute actuation with dynamic or by-pass with
176 // override actuation provided
177 addModelingOption("override_actuation", 1);
178
179 // Cache the computed actuation and speed of the scalar valued actuator
180 addCacheVariable<double>("actuation", 0.0, Stage::Velocity);
181 addCacheVariable<double>("speed", 0.0, Stage::Velocity);
182
183 // Discrete state variable is the override actuation value if in override mode
184 addDiscreteVariable("override_actuation", Stage::Time);
185 }
186
getControl(const SimTK::State & s) const187 double ScalarActuator::getControl(const SimTK::State& s) const
188 {
189 return getControls(s)[0];
190 }
191
getStress(const SimTK::State & s) const192 double ScalarActuator::getStress(const SimTK::State& s) const
193 {
194 OPENSIM_ERROR_IF_NOT_OVERRIDDEN();
195 }
196
getOptimalForce() const197 double ScalarActuator::getOptimalForce() const
198 {
199 OPENSIM_ERROR_IF_NOT_OVERRIDDEN();
200 }
201
getActuation(const State & s) const202 double ScalarActuator::getActuation(const State &s) const
203 {
204 if (appliesForce(s))
205 return getCacheVariableValue<double>(s, "actuation");
206 else
207 return 0.0;
208 }
209
setActuation(const State & s,double aActuation) const210 void ScalarActuator::setActuation(const State& s, double aActuation) const
211 {
212 setCacheVariableValue<double>(s, "actuation", aActuation);
213 }
214
getSpeed(const State & s) const215 double ScalarActuator::getSpeed(const State& s) const
216 {
217 return getCacheVariableValue<double>(s, "speed");
218 }
219
setSpeed(const State & s,double speed) const220 void ScalarActuator::setSpeed(const State &s, double speed) const
221 {
222 setCacheVariableValue<double>(s, "speed", speed);
223 }
224
overrideActuation(SimTK::State & s,bool flag) const225 void ScalarActuator::overrideActuation(SimTK::State& s, bool flag) const
226 {
227 setModelingOption(s, "override_actuation", int(flag));
228 }
229
isActuationOverridden(const SimTK::State & s) const230 bool ScalarActuator::isActuationOverridden(const SimTK::State& s) const
231 {
232 return (getModelingOption(s, "override_actuation") > 0);
233 }
234
setOverrideActuation(SimTK::State & s,double actuation) const235 void ScalarActuator::setOverrideActuation(SimTK::State& s, double actuation) const
236 {
237 setDiscreteVariableValue(s, "override_actuation", actuation);;
238 }
239
getOverrideActuation(const SimTK::State & s) const240 double ScalarActuator::getOverrideActuation(const SimTK::State& s) const
241 {
242 return getDiscreteVariableValue(s, "override_actuation");
243 }
computeOverrideActuation(const SimTK::State & s) const244 double ScalarActuator::computeOverrideActuation(const SimTK::State& s) const
245 {
246 double appliedActuation = getOverrideActuation(s);
247 setActuation(s, appliedActuation);
248 return appliedActuation;
249 }
250