1 /* -------------------------------------------------------------------------- *
2 * OpenSim: ActivationFiberLengthMuscle.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 "ActivationFiberLengthMuscle.h"
28
29 using namespace std;
30 using namespace OpenSim;
31 using SimTK::Vec3;
32
33
34 //==============================================================================
35 // STATIC INITIALIZATION
36 //==============================================================================
37 const string ActivationFiberLengthMuscle::STATE_ACTIVATION_NAME = "activation";
38 const string ActivationFiberLengthMuscle::STATE_FIBER_LENGTH_NAME = "fiber_length";
39
40
41 //==============================================================================
42 // CONSTRUCTORS
43 //==============================================================================
44 // Uses default (compiler-generated) destructor, copy constructor, and copy
45 // assignment operator.
46
47 //_____________________________________________________________________________
48 // Default constructor.
ActivationFiberLengthMuscle()49 ActivationFiberLengthMuscle::ActivationFiberLengthMuscle()
50 {
51 constructProperties();
52 }
53
54 //_____________________________________________________________________________
55 // Allocate and initialize properties.
constructProperties()56 void ActivationFiberLengthMuscle::constructProperties()
57 {
58 constructProperty_default_activation(0.05);
59 constructProperty_default_fiber_length(getOptimalFiberLength());
60 }
61
62 //_____________________________________________________________________________
63 // Allocate Simbody System resources for this actuator.
extendAddToSystem(SimTK::MultibodySystem & system) const64 void ActivationFiberLengthMuscle::extendAddToSystem(SimTK::MultibodySystem& system) const
65 {
66 Super::extendAddToSystem(system);
67 const string& className = getConcreteClassName();
68 const string& suffix = " flag is not currently implemented.";
69
70 if(get_ignore_activation_dynamics()){
71 string errMsg = className + "::ignore_activation_dynamics" + suffix;
72 throw Exception(errMsg);
73 }
74
75 if(get_ignore_tendon_compliance()){
76 string errMsg = className + "::ignore_tendon_compliance" + suffix;
77 throw Exception(errMsg);
78 }
79
80 addStateVariable(STATE_ACTIVATION_NAME);
81 // Fiber length should be a position stage state variable.
82 // That is setting the fiber length should force position and above
83 // dependent cache to be reevaluated. Problem with doing this now
84 // is that there are no position level Z variables and making it
85 // a Q (multibody position coordinate) will invalidate the whole
86 // multibody position realization which is overkill and would
87 // also wipe out the muscle path, which we do not want to
88 // reevaluate over and over.
89 addStateVariable(STATE_FIBER_LENGTH_NAME);//, SimTK::Stage::Velocity);
90 }
91
extendInitStateFromProperties(SimTK::State & s) const92 void ActivationFiberLengthMuscle::extendInitStateFromProperties( SimTK::State& s) const
93 {
94 Super::extendInitStateFromProperties(s); // invoke superclass implementation
95
96 setActivation(s, getDefaultActivation());
97 setFiberLength(s, getDefaultFiberLength());
98 }
99
extendSetPropertiesFromState(const SimTK::State & state)100 void ActivationFiberLengthMuscle::extendSetPropertiesFromState(const SimTK::State& state)
101 {
102 Super::extendSetPropertiesFromState(state); // invoke superclass implementation
103
104 setDefaultActivation(getStateVariableValue(state, STATE_ACTIVATION_NAME));
105 setDefaultFiberLength(getStateVariableValue(state, STATE_FIBER_LENGTH_NAME));
106 }
107
extendConnectToModel(Model & aModel)108 void ActivationFiberLengthMuscle::extendConnectToModel(Model& aModel)
109 {
110 Super::extendConnectToModel(aModel);
111 }
112
getDefaultActivation() const113 double ActivationFiberLengthMuscle::getDefaultActivation() const {
114 return get_default_activation();
115 }
setDefaultActivation(double activation)116 void ActivationFiberLengthMuscle::setDefaultActivation(double activation) {
117 set_default_activation(activation);
118 }
getDefaultFiberLength() const119 double ActivationFiberLengthMuscle::getDefaultFiberLength() const {
120 return get_default_fiber_length();
121 }
setDefaultFiberLength(double length)122 void ActivationFiberLengthMuscle::setDefaultFiberLength(double length) {
123 set_default_fiber_length(length);
124 }
125
126 /**
127 * Compute the derivatives of the muscle states.
128 *
129 * @param s system state
130 */
131 void ActivationFiberLengthMuscle::
computeStateVariableDerivatives(const SimTK::State & s) const132 computeStateVariableDerivatives(const SimTK::State &s) const
133 {
134 double adot = 0;
135 double ldot = 0;
136
137 if (appliesForce(s) && !isActuationOverridden(s)) {
138 adot = getActivationRate(s);
139 ldot = getFiberVelocity(s);
140 }
141
142 setStateVariableDerivativeValue(s, STATE_ACTIVATION_NAME, adot);
143 setStateVariableDerivativeValue(s, STATE_FIBER_LENGTH_NAME, ldot);
144 }
145 //==============================================================================
146 // GET
147 //==============================================================================
148 //-----------------------------------------------------------------------------
149 // STATE VALUES
150 //-----------------------------------------------------------------------------
151
setActivation(SimTK::State & s,double activation) const152 void ActivationFiberLengthMuscle::setActivation(SimTK::State& s, double activation) const
153 {
154 setStateVariableValue(s, STATE_ACTIVATION_NAME, activation);
155 }
156
setFiberLength(SimTK::State & s,double fiberLength) const157 void ActivationFiberLengthMuscle::setFiberLength(SimTK::State& s, double fiberLength) const
158 {
159 setStateVariableValue(s, STATE_FIBER_LENGTH_NAME, fiberLength);
160 // NOTE: This is a temporary measure since we were forced to allocate
161 // fiber length as a Dynamics stage dependent state variable.
162 // In order to force the recalculation of the length cache we have to
163 // invalidate the length info whenever fiber length is set.
164 markCacheVariableInvalid(s,"lengthInfo");
165 markCacheVariableInvalid(s,"velInfo");
166 markCacheVariableInvalid(s,"dynamicsInfo");
167 }
168
getActivationRate(const SimTK::State & s) const169 double ActivationFiberLengthMuscle::getActivationRate(const SimTK::State& s) const
170 {
171 return calcActivationRate(s);
172 }
173
174
175 //==============================================================================
176 // SCALING
177 //==============================================================================
178 void ActivationFiberLengthMuscle::
extendPostScale(const SimTK::State & s,const ScaleSet & scaleSet)179 extendPostScale(const SimTK::State& s, const ScaleSet& scaleSet)
180 {
181 Super::extendPostScale(s, scaleSet);
182
183 GeometryPath& path = upd_GeometryPath();
184 if (path.getPreScaleLength(s) > 0.0)
185 {
186 double scaleFactor = path.getLength(s) / path.getPreScaleLength(s);
187 upd_optimal_fiber_length() *= scaleFactor;
188 upd_tendon_slack_length() *= scaleFactor;
189
190 // Clear the pre-scale length that was stored in the GeometryPath.
191 path.setPreScaleLength(s, 0.0);
192 }
193 }
194
195 //--------------------------------------------------------------------------
196 // COMPUTATIONS
197 //--------------------------------------------------------------------------
198 //The concrete classes are the only ones qualified to write this routine.
199 /*void ActivationFiberLengthMuscle::computeInitialFiberEquilibrium(SimTK::State& s) const
200 {
201 // Reasonable initial activation value
202 //cout << getName() << "'s activation is: " << getActivation(s) << endl;
203 //cout << getName() << "'s fiber-length is: " << getFiberLength(s) << endl;
204 if(getActivation(s) < 0.01){
205 setActivation(s, 0.01);
206 }
207 //cout << getName() << "'s NEW activation is: " << getActivation(s) << endl;
208 setFiberLength(s, getOptimalFiberLength());
209 _model->getMultibodySystem().realize(s, SimTK::Stage::Velocity);
210 double force = computeIsometricForce(s, getActivation(s));
211 //cout << getName() << "'s Equilibrium activation is: " << getActivation(s) << endl;
212 //cout << getName() << "'s Equilibrium fiber-length is: " << getFiberLength(s) << endl;
213 }*/
214
215 //==============================================================================
216 // FORCE APPLICATION
217 //==============================================================================
218 //_____________________________________________________________________________
219 /**
220 * Apply the muscle's force at its points of attachment to the bodies.
221 */
computeForce(const SimTK::State & s,SimTK::Vector_<SimTK::SpatialVec> & bodyForces,SimTK::Vector & generalizedForces) const222 void ActivationFiberLengthMuscle::computeForce(const SimTK::State& s,
223 SimTK::Vector_<SimTK::SpatialVec>& bodyForces,
224 SimTK::Vector& generalizedForces) const
225 {
226 Muscle::computeForce(s, bodyForces, generalizedForces);
227
228 if (isActuationOverridden(s)) {
229 // Also define the state derivatives, since realize acceleration will
230 // ask for muscle derivatives, which will be integrated
231 // in the case the actuation is being overridden, the states aren't
232 // being used but a valid derivative cache entry is still required
233 setStateVariableDerivativeValue(s, STATE_ACTIVATION_NAME, 0.0);
234 setStateVariableDerivativeValue(s, STATE_FIBER_LENGTH_NAME, 0.0);
235 }
236 }
237