1 /* -------------------------------------------------------------------------- *
2 * OpenSim: ElasticFoundationForce.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): Peter Eastman *
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 #include "ElasticFoundationForce.h"
25 #include "ContactGeometry.h"
26 #include "ContactMesh.h"
27 #include "Model.h"
28
29 #include "simbody/internal/ElasticFoundationForce.h"
30
31 namespace OpenSim {
32
33 //==============================================================================
34 // ELASTIC FOUNDATION FORCE
35 //==============================================================================
36
37 // Default constructor.
ElasticFoundationForce()38 ElasticFoundationForce::ElasticFoundationForce()
39 {
40 constructProperties();
41 }
42
43 // Construct with supplied contact parameters.
ElasticFoundationForce(ContactParameters * params)44 ElasticFoundationForce::ElasticFoundationForce(ContactParameters* params)
45 {
46 constructProperties();
47 addContactParameters(params);
48 }
49
extendAddToSystem(SimTK::MultibodySystem & system) const50 void ElasticFoundationForce::extendAddToSystem(SimTK::MultibodySystem& system) const
51 {
52 Super::extendAddToSystem(system);
53
54 const ContactParametersSet& contactParametersSet =
55 get_contact_parameters();
56 const double& transitionVelocity = get_transition_velocity();
57
58 SimTK::GeneralContactSubsystem& contacts = system.updContactSubsystem();
59 SimTK::ContactSetIndex set = contacts.createContactSet();
60 SimTK::ElasticFoundationForce force(_model->updForceSubsystem(), contacts, set);
61 force.setTransitionVelocity(transitionVelocity);
62 for (int i = 0; i < contactParametersSet.getSize(); ++i)
63 {
64 ContactParameters& params = contactParametersSet.get(i);
65 for (int j = 0; j < params.getGeometry().size(); ++j) {
66 // TODO: Dependency of ElasticFoundationForce on ContactGeometry
67 // should be handled by Sockets.
68 const ContactGeometry* contactGeom = nullptr;
69 if (getModel().hasComponent<ContactGeometry>(params.getGeometry()[j]))
70 contactGeom = &getModel().getComponent<ContactGeometry>(
71 params.getGeometry()[j]);
72 else
73 contactGeom = &getModel().getComponent<ContactGeometry>(
74 "./contactgeometryset/" + params.getGeometry()[j]);
75
76 const ContactGeometry& geom = *contactGeom;
77 // B: base Frame (Body or Ground)
78 // F: PhysicalFrame that this ContactGeometry is connected to
79 // P: the frame defined (relative to F) by the location and
80 // orientation properties.
81 const auto& X_BF = geom.getFrame().findTransformInBaseFrame();
82 const auto& X_FP = geom.getTransform();
83 const auto X_BP = X_BF * X_FP;
84 contacts.addBody(set, geom.getFrame().getMobilizedBody(),
85 geom.createSimTKContactGeometry(), X_BP);
86 if (dynamic_cast<const ContactMesh*>(&geom) != NULL) {
87 force.setBodyParameters(
88 SimTK::ContactSurfaceIndex(contacts.getNumBodies(set)-1),
89 params.getStiffness(), params.getDissipation(),
90 params.getStaticFriction(),
91 params.getDynamicFriction(),
92 params.getViscousFriction());
93 }
94 }
95 }
96
97 // Beyond the const Component get the index so we can access the SimTK::Force later
98 ElasticFoundationForce* mutableThis = const_cast<ElasticFoundationForce *>(this);
99 mutableThis->_index = force.getForceIndex();
100 }
101
constructProperties()102 void ElasticFoundationForce::constructProperties()
103 {
104 constructProperty_contact_parameters(ContactParametersSet());
105 constructProperty_transition_velocity(0.01);
106 }
107
108
updContactParametersSet()109 ElasticFoundationForce::ContactParametersSet& ElasticFoundationForce::updContactParametersSet()
110 {
111 return upd_contact_parameters();
112 }
113
getContactParametersSet()114 const ElasticFoundationForce::ContactParametersSet& ElasticFoundationForce::getContactParametersSet()
115 {
116 return get_contact_parameters();
117 }
118
addContactParameters(ElasticFoundationForce::ContactParameters * params)119 void ElasticFoundationForce::addContactParameters(ElasticFoundationForce::ContactParameters* params)
120 {
121 updContactParametersSet().adoptAndAppend(params);
122 }
123
getTransitionVelocity() const124 double ElasticFoundationForce::getTransitionVelocity() const
125 {
126 return get_transition_velocity();
127 }
128
setTransitionVelocity(double velocity)129 void ElasticFoundationForce::setTransitionVelocity(double velocity)
130 {
131 set_transition_velocity(velocity);
132 }
133
134 /* The following set of functions are introduced for convenience to get/set values in ElasticFoundationForce::ContactParameters
135 * and for access in Matlab without exposing ElasticFoundationForce::ContactParameters. pending refactoring contact forces
136 */
getStiffness()137 double ElasticFoundationForce::getStiffness() {
138 if (get_contact_parameters().getSize()==0)
139 updContactParametersSet().adoptAndAppend(
140 new ElasticFoundationForce::ContactParameters());
141 return get_contact_parameters().get(0).getStiffness();
142 };
setStiffness(double stiffness)143 void ElasticFoundationForce::setStiffness(double stiffness) {
144 if (get_contact_parameters().getSize()==0)
145 updContactParametersSet().adoptAndAppend(
146 new ElasticFoundationForce::ContactParameters());
147 upd_contact_parameters()[0].setStiffness(stiffness);
148 };
getDissipation()149 double ElasticFoundationForce::getDissipation() {
150 if (get_contact_parameters().getSize()==0)
151 updContactParametersSet().adoptAndAppend(
152 new ElasticFoundationForce::ContactParameters());
153 return get_contact_parameters().get(0).getDissipation();
154 };
setDissipation(double dissipation)155 void ElasticFoundationForce::setDissipation(double dissipation) {
156 if (get_contact_parameters().getSize()==0)
157 updContactParametersSet().adoptAndAppend(
158 new ElasticFoundationForce::ContactParameters());
159 upd_contact_parameters()[0].setDissipation(dissipation);
160 };
getStaticFriction()161 double ElasticFoundationForce::getStaticFriction() {
162 if (get_contact_parameters().getSize()==0)
163 updContactParametersSet().adoptAndAppend(
164 new ElasticFoundationForce::ContactParameters());
165 return get_contact_parameters().get(0).getStaticFriction();
166 };
setStaticFriction(double friction)167 void ElasticFoundationForce::setStaticFriction(double friction) {
168 if (get_contact_parameters().getSize()==0)
169 updContactParametersSet().adoptAndAppend(
170 new ElasticFoundationForce::ContactParameters());
171 upd_contact_parameters()[0].setStaticFriction(friction);
172 };
getDynamicFriction()173 double ElasticFoundationForce::getDynamicFriction() {
174 if (get_contact_parameters().getSize()==0)
175 updContactParametersSet().adoptAndAppend(
176 new ElasticFoundationForce::ContactParameters());
177 return get_contact_parameters().get(0).getDynamicFriction();
178 };
setDynamicFriction(double friction)179 void ElasticFoundationForce::setDynamicFriction(double friction) {
180 if (get_contact_parameters().getSize()==0)
181 updContactParametersSet().adoptAndAppend(
182 new ElasticFoundationForce::ContactParameters());
183 upd_contact_parameters()[0].setDynamicFriction(friction);
184 };
getViscousFriction()185 double ElasticFoundationForce::getViscousFriction() {
186 if (get_contact_parameters().getSize()==0)
187 updContactParametersSet().adoptAndAppend(
188 new ElasticFoundationForce::ContactParameters());
189 return get_contact_parameters().get(0).getViscousFriction();
190 };
setViscousFriction(double friction)191 void ElasticFoundationForce::setViscousFriction(double friction) {
192 if (get_contact_parameters().getSize()==0)
193 updContactParametersSet().adoptAndAppend(
194 new ElasticFoundationForce::ContactParameters());
195 upd_contact_parameters()[0].setViscousFriction(friction);
196 };
197
addGeometry(const std::string & name)198 void ElasticFoundationForce::addGeometry(const std::string& name)
199 {
200 if (get_contact_parameters().getSize()==0)
201 updContactParametersSet().adoptAndAppend(
202 new ElasticFoundationForce::ContactParameters());
203 upd_contact_parameters()[0].addGeometry(name);
204 }
205
206 //==============================================================================
207 // ELASTIC FOUNDATION FORCE :: CONTACT PARAMETERS
208 //==============================================================================
209
210 // Default constructor.
ContactParameters()211 ElasticFoundationForce::ContactParameters::ContactParameters()
212 {
213 constructProperties();
214 }
215
216 // Constructor specifying material properties.
ContactParameters(double stiffness,double dissipation,double staticFriction,double dynamicFriction,double viscousFriction)217 ElasticFoundationForce::ContactParameters::ContactParameters
218 (double stiffness, double dissipation, double staticFriction,
219 double dynamicFriction, double viscousFriction)
220 {
221 constructProperties();
222 set_stiffness(stiffness);
223 set_dissipation(dissipation);
224 set_static_friction(staticFriction);
225 set_dynamic_friction(dynamicFriction);
226 set_viscous_friction(viscousFriction);
227 }
228
229
constructProperties()230 void ElasticFoundationForce::ContactParameters::constructProperties()
231 {
232 constructProperty_geometry(); // a list of strings
233 constructProperty_stiffness(0.0);
234 constructProperty_dissipation(0.0);
235 constructProperty_static_friction(0.0);
236 constructProperty_dynamic_friction(0.0);
237 constructProperty_viscous_friction(0.0);
238 }
239
getGeometry() const240 const Property<std::string>& ElasticFoundationForce::ContactParameters::getGeometry() const
241 {
242 return getProperty_geometry();
243 }
244
updGeometry()245 Property<std::string>& ElasticFoundationForce::ContactParameters::updGeometry()
246 {
247 return updProperty_geometry();
248 }
249
addGeometry(const std::string & name)250 void ElasticFoundationForce::ContactParameters::addGeometry(const std::string& name)
251 {
252 updGeometry().appendValue(name);
253 }
254
getStiffness() const255 double ElasticFoundationForce::ContactParameters::getStiffness() const
256 {
257 return get_stiffness();
258 }
259
setStiffness(double stiffness)260 void ElasticFoundationForce::ContactParameters::setStiffness(double stiffness)
261 {
262 set_stiffness(stiffness);
263 }
264
getDissipation() const265 double ElasticFoundationForce::ContactParameters::getDissipation() const
266 {
267 return get_dissipation();
268 }
269
setDissipation(double dissipation)270 void ElasticFoundationForce::ContactParameters::setDissipation(double dissipation)
271 {
272 set_dissipation(dissipation);
273 }
274
getStaticFriction() const275 double ElasticFoundationForce::ContactParameters::getStaticFriction() const
276 {
277 return get_static_friction();
278 }
279
setStaticFriction(double friction)280 void ElasticFoundationForce::ContactParameters::setStaticFriction(double friction)
281 {
282 set_static_friction(friction);
283 }
284
getDynamicFriction() const285 double ElasticFoundationForce::ContactParameters::getDynamicFriction() const
286 {
287 return get_dynamic_friction();
288 }
289
setDynamicFriction(double friction)290 void ElasticFoundationForce::ContactParameters::setDynamicFriction(double friction)
291 {
292 set_dynamic_friction(friction);
293 }
294
getViscousFriction() const295 double ElasticFoundationForce::ContactParameters::getViscousFriction() const
296 {
297 return get_viscous_friction();
298 }
299
setViscousFriction(double friction)300 void ElasticFoundationForce::ContactParameters::setViscousFriction(double friction)
301 {
302 set_viscous_friction(friction);
303 }
304
setNull()305 void ElasticFoundationForce::ContactParametersSet::setNull()
306 {
307 setAuthors("Peter Eastman");
308 }
309
ContactParametersSet()310 ElasticFoundationForce::ContactParametersSet::ContactParametersSet()
311 {
312 setNull();
313 }
314
315
316 //=============================================================================
317 // Reporting
318 //=============================================================================
319 /* Provide names of the quantities (column labels) of the force value(s)
320 * to be reported. */
getRecordLabels() const321 OpenSim::Array<std::string> ElasticFoundationForce::getRecordLabels() const
322 {
323 OpenSim::Array<std::string> labels("");
324
325 const ContactParametersSet& contactParametersSet =
326 get_contact_parameters();
327
328 for (int i = 0; i < contactParametersSet.getSize(); ++i)
329 {
330 ContactParameters& params = contactParametersSet.get(i);
331 for (int j = 0; j < params.getGeometry().size(); ++j)
332 {
333 // TODO: Dependency of ElasticFoundationForce on ContactGeometry
334 // should be handled by Sockets.
335 const ContactGeometry* contactGeom = nullptr;
336 if (getModel().hasComponent<ContactGeometry>(params.getGeometry()[j]))
337 contactGeom = &getModel().getComponent<ContactGeometry>(
338 params.getGeometry()[j]);
339 else
340 contactGeom = &getModel().getComponent<ContactGeometry>(
341 "./contactgeometryset/" + params.getGeometry()[j]);
342
343 const ContactGeometry& geom = *contactGeom;
344 std::string frameName = geom.getFrame().getName();
345 labels.append(getName()+"."+frameName+".force.X");
346 labels.append(getName()+"."+frameName+".force.Y");
347 labels.append(getName()+"."+frameName+".force.Z");
348 labels.append(getName()+"."+frameName+".torque.X");
349 labels.append(getName()+"."+frameName+".torque.Y");
350 labels.append(getName()+"."+frameName+".torque.Z");
351 }
352 }
353
354 return labels;
355 }
356 /*
357 * Provide the value(s) to be reported that correspond to the labels
358 */
getRecordValues(const SimTK::State & state) const359 OpenSim::Array<double> ElasticFoundationForce::getRecordValues(const SimTK::State& state) const
360 {
361 OpenSim::Array<double> values(1);
362
363 const ContactParametersSet& contactParametersSet =
364 get_contact_parameters();
365
366 const SimTK::ElasticFoundationForce& simtkForce =
367 (SimTK::ElasticFoundationForce &)(_model->getForceSubsystem().getForce(_index));
368
369 SimTK::Vector_<SimTK::SpatialVec> bodyForces(0);
370 SimTK::Vector_<SimTK::Vec3> particleForces(0);
371 SimTK::Vector mobilityForces(0);
372
373 //get the net force added to the system contributed by the Spring
374 simtkForce.calcForceContribution(state, bodyForces, particleForces,
375 mobilityForces);
376
377 for (int i = 0; i < contactParametersSet.getSize(); ++i)
378 {
379 ContactParameters& params = contactParametersSet.get(i);
380 for (int j = 0; j < params.getGeometry().size(); ++j)
381 {
382 // TODO: Dependency of ElasticFoundationForce on ContactGeometry
383 // should be handled by Sockets.
384 const ContactGeometry* contactGeom = nullptr;
385 if (getModel().hasComponent<ContactGeometry>(params.getGeometry()[j]))
386 contactGeom = &getModel().getComponent<ContactGeometry>(
387 params.getGeometry()[j]);
388 else
389 contactGeom = &getModel().getComponent<ContactGeometry>(
390 "./contactgeometryset/" + params.getGeometry()[j]);
391
392 const ContactGeometry& geom = *contactGeom;
393
394 const auto& mbi = geom.getFrame().getMobilizedBodyIndex();
395 const auto& thisBodyForce = bodyForces(mbi);
396 SimTK::Vec3 forces = thisBodyForce[1];
397 SimTK::Vec3 torques = thisBodyForce[0];
398
399 values.append(3, &forces[0]);
400 values.append(3, &torques[0]);
401 }
402 }
403
404 return values;
405 }
406
407
408 } // end of namespace OpenSim
409