1 /* -------------------------------------------------------------------------- *
2 * OpenSim: MuscleFixedWidthPennationModel.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): Matthew Millard *
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 #include "MuscleFixedWidthPennationModel.h"
24
25 using namespace OpenSim;
26 using namespace SimTK;
27 using namespace std;
28
setNull()29 void MuscleFixedWidthPennationModel::setNull()
30 {
31 setAuthors("Matthew Millard");
32 m_parallelogramHeight = SimTK::NaN;
33 m_maximumSinPennation = SimTK::NaN;
34 m_minimumFiberLength = SimTK::NaN;
35 m_minimumFiberLengthAlongTendon = SimTK::NaN;
36 }
37
constructProperties()38 void MuscleFixedWidthPennationModel::constructProperties()
39 {
40 constructProperty_optimal_fiber_length(1.0);
41 constructProperty_pennation_angle_at_optimal(0.0);
42 constructProperty_maximum_pennation_angle(acos(0.1));
43 }
44
MuscleFixedWidthPennationModel()45 MuscleFixedWidthPennationModel::MuscleFixedWidthPennationModel()
46 {
47 setNull();
48 constructProperties();
49 }
50
51 MuscleFixedWidthPennationModel::
MuscleFixedWidthPennationModel(double optimalFiberLength,double optimalPennationAngle,double maximumPennationAngle)52 MuscleFixedWidthPennationModel(double optimalFiberLength,
53 double optimalPennationAngle,
54 double maximumPennationAngle)
55 {
56 setNull();
57 constructProperties();
58
59 set_optimal_fiber_length(optimalFiberLength);
60 set_pennation_angle_at_optimal(optimalPennationAngle);
61 set_maximum_pennation_angle(maximumPennationAngle);
62 }
63
getParallelogramHeight() const64 double MuscleFixedWidthPennationModel::getParallelogramHeight() const
65 { return m_parallelogramHeight; }
getMinimumFiberLength() const66 double MuscleFixedWidthPennationModel::getMinimumFiberLength() const
67 { return m_minimumFiberLength; }
getMinimumFiberLengthAlongTendon() const68 double MuscleFixedWidthPennationModel::getMinimumFiberLengthAlongTendon() const
69 { return m_minimumFiberLengthAlongTendon; }
70
71 double MuscleFixedWidthPennationModel::
clampFiberLength(double fiberLength) const72 clampFiberLength(double fiberLength) const
73 {
74 return max(m_minimumFiberLength, fiberLength);
75 }
76
77 //==============================================================================
78 // COMPONENT INTERFACE
79 //==============================================================================
extendFinalizeFromProperties()80 void MuscleFixedWidthPennationModel::extendFinalizeFromProperties()
81 {
82 Super::extendFinalizeFromProperties();
83
84 std::string errorLocation = getName() +
85 " MuscleFixedWidthPennationModel::extendFinalizeFromProperties";
86
87 // Ensure property values are within appropriate ranges.
88 OPENSIM_THROW_IF_FRMOBJ(
89 get_optimal_fiber_length() <= 0,
90 InvalidPropertyValue,
91 getProperty_optimal_fiber_length().getName(),
92 "Optimal fiber length must be greater than zero");
93 OPENSIM_THROW_IF_FRMOBJ(
94 get_pennation_angle_at_optimal() < 0 ||
95 get_pennation_angle_at_optimal() > SimTK::Pi/2.0-SimTK::SignificantReal,
96 InvalidPropertyValue,
97 getProperty_pennation_angle_at_optimal().getName(),
98 "Pennation angle at optimal fiber length must be in the range [0, Pi/2)");
99 OPENSIM_THROW_IF_FRMOBJ(
100 get_maximum_pennation_angle() < 0 ||
101 get_maximum_pennation_angle() > SimTK::Pi/2.0,
102 InvalidPropertyValue,
103 getProperty_maximum_pennation_angle().getName(),
104 "Maximum pennation angle must be in the range [0, Pi/2]");
105
106 // Compute quantities that are used often.
107 m_parallelogramHeight = get_optimal_fiber_length()
108 * sin(get_pennation_angle_at_optimal());
109 m_maximumSinPennation = sin(get_maximum_pennation_angle());
110
111 // Compute the minimum fiber length:
112 // - pennated: the length of the fiber when at its maximum pennation angle
113 // - straight: 1% of the optimal fiber length
114 if(get_maximum_pennation_angle() > SimTK::SignificantReal) {
115 m_minimumFiberLength = m_parallelogramHeight / m_maximumSinPennation;
116 } else {
117 m_minimumFiberLength = get_optimal_fiber_length()*0.01;
118 }
119 m_minimumFiberLengthAlongTendon = m_minimumFiberLength
120 * cos(get_maximum_pennation_angle());
121 }
122
123 //==============================================================================
124 // Position-level kinematics
125 //==============================================================================
126 double MuscleFixedWidthPennationModel::
calcPennationAngle(double fiberLength) const127 calcPennationAngle(double fiberLength) const
128 {
129 double phi = 0;
130 double optimalPennationAngle = get_pennation_angle_at_optimal();
131
132 // This computation is only worth performing on pennated muscles.
133 if(optimalPennationAngle > SimTK::Eps) {
134 if(fiberLength > m_minimumFiberLength) {
135 double sin_phi = m_parallelogramHeight/fiberLength;
136 phi = (sin_phi < m_maximumSinPennation) ?
137 asin(sin_phi) : get_maximum_pennation_angle();
138 } else {
139 phi = get_maximum_pennation_angle();
140 }
141 }
142 return phi;
143 }
144
calcTendonLength(double cosPennationAngle,double fiberLength,double muscleLength) const145 double MuscleFixedWidthPennationModel::calcTendonLength(
146 double cosPennationAngle, double fiberLength, double muscleLength) const
147 {
148 return muscleLength - fiberLength*cosPennationAngle;
149 }
150
calcFiberLengthAlongTendon(double fiberLength,double cosPennationAngle) const151 double MuscleFixedWidthPennationModel::calcFiberLengthAlongTendon(
152 double fiberLength, double cosPennationAngle) const
153 {
154 return fiberLength*cosPennationAngle;
155 }
156
157 //==============================================================================
158 // Velocity-level kinematics
159 //==============================================================================
calcPennationAngularVelocity(double tanPennationAngle,double fiberLength,double fiberVelocity) const160 double MuscleFixedWidthPennationModel::calcPennationAngularVelocity(
161 double tanPennationAngle, double fiberLength, double fiberVelocity) const
162 {
163 double dphi = 0;
164 double optimalPennationAngle = get_pennation_angle_at_optimal();
165
166 if(optimalPennationAngle > SimTK::Eps) {
167 SimTK_ERRCHK_ALWAYS(fiberLength > 0,
168 "MuscleFixedWidthPennationModel::calcPennationAngularVelocity",
169 "Fiber length cannot be zero.");
170 dphi = -(fiberVelocity/fiberLength) * tanPennationAngle;
171 }
172 return dphi;
173 }
174
175 double MuscleFixedWidthPennationModel::
calcTendonVelocity(double cosPennationAngle,double sinPennationAngle,double pennationAngularVelocity,double fiberLength,double fiberVelocity,double muscleVelocity) const176 calcTendonVelocity(double cosPennationAngle,
177 double sinPennationAngle,
178 double pennationAngularVelocity,
179 double fiberLength,
180 double fiberVelocity,
181 double muscleVelocity) const
182 {
183 return muscleVelocity - fiberVelocity*cosPennationAngle
184 + fiberLength*sinPennationAngle*pennationAngularVelocity;
185 }
186
187 double MuscleFixedWidthPennationModel::
calcFiberVelocityAlongTendon(double fiberLength,double fiberVelocity,double sinPennationAngle,double cosPennationAngle,double pennationAngularVelocity) const188 calcFiberVelocityAlongTendon(double fiberLength,
189 double fiberVelocity,
190 double sinPennationAngle,
191 double cosPennationAngle,
192 double pennationAngularVelocity) const
193 {
194 return fiberVelocity*cosPennationAngle
195 - fiberLength*sinPennationAngle*pennationAngularVelocity;
196 }
197
198 //==============================================================================
199 // Acceleration-level kinematics
200 //==============================================================================
201 double MuscleFixedWidthPennationModel::
calcPennationAngularAcceleration(double fiberLength,double fiberVelocity,double fiberAcceleration,double sinPennationAngle,double cosPennationAngle,double pennationAngularVelocity) const202 calcPennationAngularAcceleration(double fiberLength,
203 double fiberVelocity,
204 double fiberAcceleration,
205 double sinPennationAngle,
206 double cosPennationAngle,
207 double pennationAngularVelocity) const
208 {
209 SimTK_ERRCHK_ALWAYS(fiberLength > 0,
210 "MuscleFixedWidthPennationModel::calcPennationAngularAcceleration",
211 "Fiber length cannot be zero.");
212 SimTK_ERRCHK_ALWAYS(cosPennationAngle > 0,
213 "MuscleFixedWidthPennationModel::calcPennationAngularAcceleration",
214 "cosPennationAngle cannot be zero.");
215
216 double numer = sinPennationAngle*cosPennationAngle*
217 (fiberVelocity*fiberVelocity - fiberLength*fiberAcceleration)
218 - fiberLength*fiberVelocity*pennationAngularVelocity;
219 return numer / (fiberLength*fiberLength
220 * cosPennationAngle*cosPennationAngle);
221 }
222
223 double MuscleFixedWidthPennationModel::
calcFiberAccelerationAlongTendon(double fiberLength,double fiberVelocity,double fiberAcceleration,double sinPennationAngle,double cosPennationAngle,double pennationAngularVelocity,double pennationAngularAcceleration) const224 calcFiberAccelerationAlongTendon(double fiberLength,
225 double fiberVelocity,
226 double fiberAcceleration,
227 double sinPennationAngle,
228 double cosPennationAngle,
229 double pennationAngularVelocity,
230 double pennationAngularAcceleration) const
231 {
232 return fiberAcceleration*cosPennationAngle
233 - fiberLength*sinPennationAngle*pennationAngularAcceleration
234 - pennationAngularVelocity*
235 (2.0*fiberVelocity*sinPennationAngle
236 + fiberLength*cosPennationAngle*pennationAngularVelocity);
237 }
238
239 //==============================================================================
240 // Partial derivatives with respect to fiber length
241 //==============================================================================
242 double MuscleFixedWidthPennationModel::
calc_DFiberLengthAlongTendon_DfiberLength(double fiberLength,double sinPennationAngle,double cosPennationAngle,double DpennationAngle_DfiberLength) const243 calc_DFiberLengthAlongTendon_DfiberLength(double fiberLength,
244 double sinPennationAngle,
245 double cosPennationAngle,
246 double DpennationAngle_DfiberLength)
247 const
248 {
249 // d/dlce (lce*cos(phi)) = cos(phi) - lce*sin(phi)*dphi_dlce
250 return cosPennationAngle
251 - fiberLength*sinPennationAngle*DpennationAngle_DfiberLength;
252 }
253
254 double MuscleFixedWidthPennationModel::
calc_DPennationAngularVelocity_DfiberLength(double fiberLength,double fiberVelocity,double sinPennationAngle,double cosPennationAngle,double pennationAngularVelocity,double DpennationAngle_DfiberLength) const255 calc_DPennationAngularVelocity_DfiberLength(double fiberLength,
256 double fiberVelocity,
257 double sinPennationAngle,
258 double cosPennationAngle,
259 double pennationAngularVelocity,
260 double DpennationAngle_DfiberLength)
261 const
262 {
263 SimTK_ERRCHK_ALWAYS(cosPennationAngle > SimTK::Eps,
264 "MuscleFixedWidthPennationModel::"
265 "calc_DFiberVelocityAlongTendon_DfiberLength",
266 "Pennation angle must be less than 90 degrees");
267 SimTK_ERRCHK_ALWAYS(fiberLength > SimTK::Eps,
268 "MuscleFixedWidthPennationModel::"
269 "calc_DFiberVelocityAlongTendon_DfiberLength",
270 "Fiber length is close to 0.");
271
272 // dphidt = -(fiberVelocity/fiberLength)*tanPennationAngle
273 double tanPhi = sinPennationAngle/cosPennationAngle;
274 double DtanPhi_Dlce = (1+tanPhi*tanPhi)*DpennationAngle_DfiberLength;
275 return (fiberVelocity/(fiberLength*fiberLength))*tanPhi
276 - (fiberVelocity/fiberLength)*DtanPhi_Dlce;
277 }
278
279 double MuscleFixedWidthPennationModel::
calc_DFiberVelocityAlongTendon_DfiberLength(double fiberLength,double fiberVelocity,double sinPennationAngle,double cosPennationAngle,double pennationAngularVelocity,double DpennationAngle_DfiberLength,double DpennationAngularVelocity_DfiberLength) const280 calc_DFiberVelocityAlongTendon_DfiberLength(
281 double fiberLength,
282 double fiberVelocity,
283 double sinPennationAngle,
284 double cosPennationAngle,
285 double pennationAngularVelocity,
286 double DpennationAngle_DfiberLength,
287 double DpennationAngularVelocity_DfiberLength)
288 const
289 {
290 // dlceAT = dlce*cos(phi) - lce*sin(phi)*dphidt
291 // Now take the partial derivative of dlceAT with respect to lce
292 return fiberVelocity*(-sinPennationAngle*DpennationAngle_DfiberLength)
293 - sinPennationAngle*pennationAngularVelocity
294 - fiberLength*cosPennationAngle
295 *DpennationAngle_DfiberLength*pennationAngularVelocity
296 - fiberLength*sinPennationAngle
297 *DpennationAngularVelocity_DfiberLength;
298 }
299
300 double MuscleFixedWidthPennationModel::
calc_DPennationAngle_DfiberLength(double fiberLength) const301 calc_DPennationAngle_DfiberLength(double fiberLength) const
302 {
303 SimTK_ERRCHK_ALWAYS(fiberLength > m_parallelogramHeight,
304 "MuscleFixedWidthPennationModel::calc_DPennationAngle_DfiberLength",
305 "Fiber length is below the lower bound for this muscle.");
306
307 // phi = asin( parallelogramHeight/lce)
308 // d_phi/d_lce = d/dlce (asin(parallelogramHeight/lce))
309 double h_over_l = m_parallelogramHeight / fiberLength;
310 return (-h_over_l/fiberLength) / sqrt(1.0 - h_over_l*h_over_l);
311 }
312
313 double MuscleFixedWidthPennationModel::
calc_DTendonLength_DfiberLength(double fiberLength,double sinPennationAngle,double cosPennationAngle,double DpennationAngle_DfiberLength) const314 calc_DTendonLength_DfiberLength(double fiberLength,
315 double sinPennationAngle,
316 double cosPennationAngle,
317 double DpennationAngle_DfiberLength) const
318 {
319 SimTK_ERRCHK_ALWAYS(fiberLength > m_parallelogramHeight,
320 "MuscleFixedWidthPennationModel::calc_DTendonLength_DfiberLength",
321 "Fiber length is below the lower bound for this muscle.");
322
323 // dtl_dlce = DmuscleLength_DfiberLength - cosPennationAngle
324 // + fiberLength*sinPennationAngle*DpennationAngle_DfiberLength
325 return fiberLength*sinPennationAngle*DpennationAngle_DfiberLength
326 - cosPennationAngle;
327 }
328
329 //==============================================================================
330 // Kinematic fiber pose equations (useful during initialization)
331 //==============================================================================
calcFiberLength(double muscleLength,double tendonLength) const332 double MuscleFixedWidthPennationModel::calcFiberLength(double muscleLength,
333 double tendonLength) const
334 {
335 double fiberLengthAT = muscleLength-tendonLength;
336 double fiberLength = 0.0;
337
338 if(fiberLengthAT >= getMinimumFiberLengthAlongTendon()) {
339 fiberLength = sqrt(m_parallelogramHeight*m_parallelogramHeight
340 + fiberLengthAT*fiberLengthAT);
341 } else {
342 fiberLength = getMinimumFiberLength();
343 }
344 return fiberLength;
345 }
346
347 double MuscleFixedWidthPennationModel::
calcFiberVelocity(double cosPennationAngle,double muscleVelocity,double tendonVelocity) const348 calcFiberVelocity(double cosPennationAngle,
349 double muscleVelocity,
350 double tendonVelocity) const
351 {
352 // Differentiate l^M cos(phi) + l^T = l^MT ....(1)
353 // Differentiate h = l^M sin(phi) .............(2)
354 // Solve (2) for phidot, substitute into (1), and solve for v^M.
355 return (muscleVelocity-tendonVelocity)*cosPennationAngle;
356 }
357